add some initial simplistic TLV parser

2 files changed, 43 insertions, 0 deletions

diff --git a/include/openbsc/tlv.h b/include/openbsc/tlv.h
index 70d0319e8..668644be7 100644
--- a/include/openbsc/tlv.h
+++ b/include/openbsc/tlv.h
@@ -4,10 +4,14 @@
 #include <sys/types.h>
 #include <string.h>
 
+#include <openbsc/msgb.h>
+
 #define TLV_GROSS_LEN(x)	(x+2)
 #define TLV16_GROSS_LEN(x)	((2*x)+2)
 #define TL16V_GROSS_LEN(x)	(x+3)
 
+/* TLV generation */
+
 static inline u_int8_t *tlv_put(u_int8_t *buf, u_int8_t tag, u_int8_t len,
 				const u_int8_t *val)
 {
@@ -102,5 +106,19 @@ static inline u_int8_t *msgb_tv16_push(struct msgb *msg, u_int8_t tag, u_int16_t
 	return tv16_put(buf, tag, val);
 }
 
+/* TLV parsing */
+
+struct tlv_p_entry {
+	u_int8_t len;
+	u_int8_t *val;
+};
+
+struct tlv_parser {
+	struct tlv_p_entry lv[0xff];
+};
+
+#define TLVP_PRESENT(x, y)	(x->lv[y].val)
+#define TLVP_LEN(x, y)		x->lv[y].len
+#define TLVP_VAL(x, y)		x->lv[y].val
 
 #endif /* _TLV_H */
diff --git a/src/tlv_parser.c b/src/tlv_parser.c
new file mode 100644
index 000000000..182c54d24
--- /dev/null
+++ b/src/tlv_parser.c
@@ -0,0 +1,25 @@
+#include <openbsc/tlv.h>
+
+int tlv_parse(struct tlv_parser *parser, u_int8_t *data, int data_len)
+{
+	u_int8_t *cur = data;
+	memset(parser, 0, sizeof(*parser));
+
+	while (cur +2 <= data + data_len) {
+		u_int8_t tag, len;
+		u_int8_t *val;
+
+		tag = *cur++;
+		len = *cur++;
+		val = cur;
+
+		parser->lv[tag].len = len;
+		parser->lv[tag].val = val;
+
+		if (cur + len > data + data_len)
+			break;
+
+		cur += len;
+	}
+	return 0;
+}