From 9f3b44d6ff1b8f8968c5399374433439ac0fbb5b Mon Sep 17 00:00:00 2001
From: Harald Welte <laforge@osmocom.org>
Date: Tue, 4 May 2021 18:18:09 +0200
Subject: [PATCH] utils: COMPREHENSION-TLV support

Change-Id: I8d969382b73fa152ee09c456fa4aee428fb36285
---
 pySim/utils.py      | 44 ++++++++++++++++++++++++++++++++++++++++++++
 tests/test_utils.py | 22 ++++++++++++++++++++++
 2 files changed, 66 insertions(+)

diff --git a/pySim/utils.py b/pySim/utils.py
index 3d96580b..d96d05a9 100644
--- a/pySim/utils.py
+++ b/pySim/utils.py
@@ -89,6 +89,50 @@ def lpad(s:str, l:int, c='f') -> str:
 def half_round_up(n:int) -> int:
 	return (n + 1)//2
 
+#########################################################################
+# poor man's COMPREHENSION-TLV decoder.
+#########################################################################
+
+def comprehensiontlv_parse_tag(binary:bytes) -> Tuple[dict, bytes]:
+    """Parse a single Tag according to ETSI TS 101 220 Section 7.1.1"""
+    if binary[0] in [0x00, 0x80, 0xff]:
+        raise ValueError("Found illegal value 0x%02x in %s" % (binary[0], binary))
+    if binary[0] == 0x7f:
+        # three-byte tag
+        tag = (binary[1] & 0x7f) << 8
+        tag |= binary[2]
+        compr = True if binary[1] & 0x80 else False
+        return ({'comprehension': compr, 'tag': tag}, binary[3:])
+    else:
+        # single byte tag
+        tag = binary[0] & 0x7f
+        compr = True if binary[0] & 0x80 else False
+        return ({'comprehension': compr, 'tag': tag}, binary[1:])
+
+def comprehensiontlv_encode_tag(tag) -> bytes:
+    """Encode a single Tag according to ETSI TS 101 220 Section 7.1.1"""
+    # permit caller to specify tag also as integer value
+    if isinstance(tag, int):
+        compr = True if tag < 0xff and tag & 0x80 else False
+        tag = {'tag': tag, 'comprehension': compr}
+    compr = tag.get('comprehension', False)
+    if tag['tag'] in [0x00, 0x80, 0xff] or tag['tag'] > 0xff:
+        # 3-byte format
+        byte3 = tag['tag'] & 0xff;
+        byte2 = (tag['tag'] >> 8) & 0x7f
+        if compr:
+            byte2 |= 0x80
+        return b'\x7f' + byte2.to_bytes(1, 'big') + byte3.to_bytes(1, 'big')
+    else:
+        # 1-byte format
+        ret = tag['tag']
+        if compr:
+            ret |= 0x80
+        return ret.to_bytes(1, 'big')
+
+# length value coding is equal to BER-TLV
+
+
 #########################################################################
 # poor man's BER-TLV decoder. To be a more sophisticated OO library later
 #########################################################################
diff --git a/tests/test_utils.py b/tests/test_utils.py
index 17a93004..ed22eff9 100755
--- a/tests/test_utils.py
+++ b/tests/test_utils.py
@@ -179,6 +179,28 @@ class TestBerTlv(unittest.TestCase):
         res = utils.bertlv_parse_one(b'\x81\x01\x01');
         self.assertEqual(res, ({'tag':1, 'constructed':False, 'class':2}, 1, b'\x01', b''))
 
+class TestComprTlv(unittest.TestCase):
+    def test_ComprTlvTagDec(self):
+        res = utils.comprehensiontlv_parse_tag(b'\x12\x23')
+        self.assertEqual(res, ({'tag': 0x12, 'comprehension': False}, b'\x23'))
+        res = utils.comprehensiontlv_parse_tag(b'\x92')
+        self.assertEqual(res, ({'tag': 0x12, 'comprehension': True}, b''))
+        res = utils.comprehensiontlv_parse_tag(b'\x7f\x12\x34')
+        self.assertEqual(res, ({'tag': 0x1234, 'comprehension': False}, b''))
+        res = utils.comprehensiontlv_parse_tag(b'\x7f\x82\x34\x56')
+        self.assertEqual(res, ({'tag': 0x234, 'comprehension': True}, b'\x56'))
+
+    def test_ComprTlvTagEnc(self):
+        res  = utils.comprehensiontlv_encode_tag(0x12)
+        self.assertEqual(res, b'\x12')
+        res  = utils.comprehensiontlv_encode_tag({'tag': 0x12})
+        self.assertEqual(res, b'\x12')
+        res  = utils.comprehensiontlv_encode_tag({'tag': 0x12, 'comprehension':True})
+        self.assertEqual(res, b'\x92')
+        res  = utils.comprehensiontlv_encode_tag(0x1234)
+        self.assertEqual(res, b'\x7f\x12\x34')
+        res  = utils.comprehensiontlv_encode_tag({'tag': 0x1234, 'comprehension':True})
+        self.assertEqual(res, b'\x7f\x92\x34')
 
 if __name__ == "__main__":
 	unittest.main()