[TLV] extend TLV parser with support for TvLV

Tag-variableLength-Value is an encoding scheme used in the GPRS NS
and BSSGP protocols, where the length value can be 8 or 16 bits,
depending on actual demand.

2 files changed, 69 insertions, 0 deletions

diff --git a/openbsc/include/openbsc/tlv.h b/openbsc/include/openbsc/tlv.h
index ae88e6ed3..6da4fb16a 100644
--- a/openbsc/include/openbsc/tlv.h
+++ b/openbsc/include/openbsc/tlv.h
@@ -6,11 +6,33 @@
 
 #include <openbsc/msgb.h>
 
+/* Terminology / wording
+		tag	length		value	(in bits)
+
+	    V	-	-		8
+	   LV	-	8		N * 8
+	  TLV	8	8		N * 8
+	TL16V	8	16		N * 8
+	TLV16	8	8		N * 16
+	 TvLV	8	8/16		N * 8
+
+*/
+
 #define LV_GROSS_LEN(x)		(x+1)
 #define TLV_GROSS_LEN(x)	(x+2)
 #define TLV16_GROSS_LEN(x)	((2*x)+2)
 #define TL16V_GROSS_LEN(x)	(x+3)
 
+#define TVLV_MAX_ONEBYTE	0x7f
+
+static inline u_int16_t TVLV_GROSS_LEN(u_int16_t len)
+{
+	if (len <= TVLV_MAX_ONEBYTE)
+		return TLV_GROSS_LEN(len);
+	else
+		return TL16V_GROSS_LEN(len);
+}
+
 /* TLV generation */
 
 static inline u_int8_t *lv_put(u_int8_t *buf, u_int8_t len,
@@ -49,6 +71,20 @@ static inline u_int8_t *tl16v_put(u_int8_t *buf, u_int8_t tag, u_int16_t len,
 	return buf + len*2;
 }
 
+static inline u_int8_t *tvlv_put(u_int8_t *buf, u_int8_t tag, u_int16_t len,
+				 const u_int8_t *val)
+{
+	u_int8_t *ret;
+
+	if (len <= TVLV_MAX_ONEBYTE) {
+		ret = tlv_put(buf, tag, len, val);
+		buf[1] |= 0x80;
+	} else
+		ret = tl16v_put(buf, tag, len, val);
+
+	return ret;
+}
+
 static inline u_int8_t *msgb_tlv16_put(struct msgb *msg, u_int8_t tag, u_int8_t len, const u_int16_t *val)
 {
 	u_int8_t *buf = msgb_put(msg, TLV16_GROSS_LEN(len));
@@ -62,6 +98,13 @@ static inline u_int8_t *msgb_tl16v_put(struct msgb *msg, u_int8_t tag, u_int16_t
 	return tl16v_put(buf, tag, len, val);
 }
 
+static inline u_int8_t *msgb_tvlv_put(struct msgb *msg, u_int8_t tag, u_int16_t len,
+				      const u_int8_t *val)
+{
+	u_int8_t *buf = msgb_put(msg, TVLV_GROSS_LEN(len));
+	return tvlv_put(buf, tag, len, val);
+}
+
 static inline u_int8_t *v_put(u_int8_t *buf, u_int8_t val)
 {
 	*buf++ = val;
@@ -146,6 +189,7 @@ enum tlv_type {
 	TLV_TYPE_TV,
 	TLV_TYPE_TLV,
 	TLV_TYPE_TL16V,
+	TLV_TYPE_TvLV,
 };
 
 struct tlv_def {
@@ -161,6 +205,8 @@ struct tlv_parsed {
 	struct tlv_p_entry lv[0xff];
 };
 
+extern struct tlv_definition tvlv_att_def;
+
 int tlv_parse(struct tlv_parsed *dec, const struct tlv_definition *def,
 	      const u_int8_t *buf, int buf_len, u_int8_t lv_tag, u_int8_t lv_tag2);
 
diff --git a/openbsc/src/tlv_parser.c b/openbsc/src/tlv_parser.c
index e835f951f..8321b880e 100644
--- a/openbsc/src/tlv_parser.c
+++ b/openbsc/src/tlv_parser.c
@@ -1,5 +1,8 @@
 #include <stdio.h>
 #include <openbsc/tlv.h>
+#include <openbsc/gsm_data.h>
+
+struct tlv_definition tvlv_att_def;
 
 int tlv_dump(struct tlv_parsed *dec)
 {
@@ -87,6 +90,20 @@ int tlv_parse(struct tlv_parsed *dec, const struct tlv_definition *def,
 				return -2;
 			num_parsed++;
 			break;
+		case TLV_TYPE_TvLV:
+			if (*(pos+1) & 0x80) {
+				/* like TLV, but without highest bit of len */
+				if (pos + 1 > buf + buf_len)
+					return -1;
+				dec->lv[tag].val = pos+2;
+				dec->lv[tag].len = *(pos+1) & 0x7f;
+				len = dec->lv[tag].len + 2;
+				if (pos + len > buf + buf_len)
+					return -2;
+				num_parsed++;
+				break;
+			}
+			/* like TL16V, fallthrough */
 		case TLV_TYPE_TL16V:
 			if (pos + 2 > buf + buf_len)
 				return -1;
@@ -103,3 +120,9 @@ int tlv_parse(struct tlv_parsed *dec, const struct tlv_definition *def,
 	return num_parsed;
 }
 
+static __attribute__((constructor)) void on_dso_load_tlv(void)
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(tvlv_att_def.def); i++)
+		tvlv_att_def.def[i].type = TLV_TYPE_TvLV;
+}