1 files changed, 116 insertions, 0 deletions
diff --git a/openbsc/src/gprs/gb_proxy.c b/openbsc/src/gprs/gb_proxy.c
index bc2e119eb..bf79444a6 100644
--- a/openbsc/src/gprs/gb_proxy.c
+++ b/openbsc/src/gprs/gb_proxy.c
@@ -403,6 +403,122 @@ int gbprox_str_to_apn(uint8_t *apn_enc, const char *str, size_t max_chars)
 	return len;
 }
 
+/* TODO: Move shift functions to libosmocore */
+
+int v_fixed_shift(uint8_t **data, size_t *data_len,
+		  size_t len, uint8_t **value)
+{
+	if (len > *data_len)
+		goto fail;
+
+	if (value)
+		*value = *data;
+
+	*data += len;
+	*data_len -= len;
+
+	return len;
+
+fail:
+	*data += *data_len;
+	*data_len = 0;
+	return -1;
+}
+
+int tv_fixed_match(uint8_t **data, size_t *data_len,
+		   uint8_t tag, size_t len,
+		   uint8_t **value)
+{
+	size_t ie_len;
+
+	if (*data_len == 0)
+		goto fail;
+
+	if ((*data)[0] != tag)
+		return 0;
+
+	if (len > *data_len - 1)
+		goto fail;
+
+	if (value)
+		*value = *data + 1;
+
+	ie_len = len + 1;
+	*data += ie_len;
+	*data_len -= ie_len;
+
+	return ie_len;
+
+fail:
+	*data += *data_len;
+	*data_len = 0;
+	return -1;
+}
+
+int tlv_match(uint8_t **data, size_t *data_len,
+	      uint8_t tag, uint8_t **value, size_t *value_len)
+{
+	size_t len;
+	size_t ie_len;
+
+	if (*data_len < 2)
+		goto fail;
+
+	if ((*data)[0] != tag)
+		return 0;
+
+	len = (*data)[1];
+	if (len > *data_len - 2)
+		goto fail;
+
+	if (value)
+		*value = *data + 2;
+	if (value_len)
+		*value_len = len;
+
+	ie_len = len + 2;
+
+	*data += ie_len;
+	*data_len -= ie_len;
+
+	return ie_len;
+
+fail:
+	*data += *data_len;
+	*data_len = 0;
+	return -1;
+}
+
+int lv_shift(uint8_t **data, size_t *data_len,
+	     uint8_t **value, size_t *value_len)
+{
+	size_t len;
+	size_t ie_len;
+
+	if (*data_len < 1)
+		goto fail;
+
+	len = (*data)[0];
+	if (len > *data_len - 1)
+		goto fail;
+
+	if (value)
+		*value = *data + 1;
+	if (value_len)
+		*value_len = len;
+
+	ie_len = len + 1;
+	*data += ie_len;
+	*data_len -= ie_len;
+
+	return ie_len;
+
+fail:
+	*data += *data_len;
+	*data_len = 0;
+	return -1;
+}
+
 struct gbprox_tlli_info {
 	struct llist_head list;