gprs: Add gprs_shift_tlv function

This function is similar to gprs_match_tlv with the exception, that
the tag is not compared but returned in *tag instead.

Sponsored-by: On-Waves ehf

3 files changed, 42 insertions, 9 deletions

diff --git a/openbsc/include/openbsc/gprs_utils.h b/openbsc/include/openbsc/gprs_utils.h
index 60d0ef6f5..60b55a178 100644
--- a/openbsc/include/openbsc/gprs_utils.h
+++ b/openbsc/include/openbsc/gprs_utils.h
@@ -40,6 +40,8 @@ int gprs_shift_v_fixed(uint8_t **data, size_t *data_len,
 		       size_t len, uint8_t **value);
 int gprs_match_tv_fixed(uint8_t **data, size_t *data_len,
 			uint8_t tag, size_t len, uint8_t **value);
+int gprs_shift_tlv(uint8_t **data, size_t *data_len,
+		   uint8_t *tag, uint8_t **value, size_t *value_len);
 int gprs_match_tlv(uint8_t **data, size_t *data_len,
 		   uint8_t tag, uint8_t **value, size_t *value_len);
 int gprs_shift_lv(uint8_t **data, size_t *data_len,
diff --git a/openbsc/src/gprs/gprs_utils.c b/openbsc/src/gprs/gprs_utils.c
index cec07d7ec..55bc629e8 100644
--- a/openbsc/src/gprs/gprs_utils.c
+++ b/openbsc/src/gprs/gprs_utils.c
@@ -271,7 +271,26 @@ fail:
 }
 
 int gprs_match_tlv(uint8_t **data, size_t *data_len,
-	      uint8_t tag, uint8_t **value, size_t *value_len)
+	      uint8_t expected_tag, uint8_t **value, size_t *value_len)
+{
+	int rc;
+	uint8_t tag;
+	uint8_t *old_data = *data;
+	size_t old_data_len = *data_len;
+
+	rc = gprs_shift_tlv(data, data_len, &tag, value, value_len);
+
+	if (rc > 0 && tag != expected_tag) {
+		*data = old_data;
+		*data_len = old_data_len;
+		return 0;
+	}
+
+	return rc;
+}
+
+int gprs_shift_tlv(uint8_t **data, size_t *data_len,
+	      uint8_t *tag, uint8_t **value, size_t *value_len)
 {
 	size_t len;
 	size_t ie_len;
@@ -279,13 +298,12 @@ int gprs_match_tlv(uint8_t **data, size_t *data_len,
 	if (*data_len < 2)
 		goto fail;
 
-	if ((*data)[0] != tag)
-		return 0;
-
 	len = (*data)[1];
 	if (len > *data_len - 2)
 		goto fail;
 
+	if (tag)
+		*tag = (*data)[0];
 	if (value)
 		*value = *data + 2;
 	if (value_len)
diff --git a/openbsc/tests/gprs/gprs_test.c b/openbsc/tests/gprs/gprs_test.c
index 5bc207343..1b20db147 100644
--- a/openbsc/tests/gprs/gprs_test.c
+++ b/openbsc/tests/gprs/gprs_test.c
@@ -139,20 +139,33 @@ static void test_gsm_03_03_apn(void)
 }
 
 /* TODO: Move tlv testing to libosmocore */
-static void check_tlv_match(uint8_t **data, size_t *data_len,
-			    uint8_t tag, size_t exp_len, const uint8_t *exp_val)
+static void check_tlv_parse(uint8_t **data, size_t *data_len,
+			    uint8_t exp_tag, size_t exp_len, const uint8_t *exp_val)
 {
 	uint8_t *value;
 	size_t value_len;
+	uint8_t tag;
 	int rc;
+	uint8_t *saved_data = *data;
+	size_t saved_data_len = *data_len;
 
-	rc = gprs_match_tlv(data, data_len, tag ^ 1, NULL, NULL);
+	rc = gprs_match_tlv(data, data_len, exp_tag ^ 1, NULL, NULL);
 	OSMO_ASSERT(rc == 0);
 
-	rc = gprs_match_tlv(data, data_len, tag, &value, &value_len);
+	rc = gprs_match_tlv(data, data_len, exp_tag, &value, &value_len);
 	OSMO_ASSERT(rc == (int)value_len + 2);
 	OSMO_ASSERT(value_len == exp_len);
 	OSMO_ASSERT(memcmp(value, exp_val, exp_len) == 0);
+
+	/* restore data/data_len */
+	*data = saved_data;
+	*data_len = saved_data_len;
+
+	rc = gprs_shift_tlv(data, data_len, &tag, &value, &value_len);
+	OSMO_ASSERT(rc == (int)value_len + 2);
+	OSMO_ASSERT(tag == exp_tag);
+	OSMO_ASSERT(value_len == exp_len);
+	OSMO_ASSERT(memcmp(value, exp_val, exp_len) == 0);
 }
 
 static void check_tv_fixed_match(uint8_t **data, size_t *data_len,
@@ -340,7 +353,7 @@ static void test_tlv_shift_functions()
 		OSMO_ASSERT(data_len <= sizeof(buf));
 
 		for (i = 0; i < iterations; i++) {
-			check_tlv_match(&data, &data_len, tag, len, test_data);
+			check_tlv_parse(&data, &data_len, tag, len, test_data);
 			check_tv_fixed_match(&data, &data_len, tag, len, test_data);
 			check_v_fixed_shift(&data, &data_len, len, test_data);
 			check_lv_shift(&data, &data_len, len, test_data);