1 files changed, 117 insertions, 0 deletions
diff --git a/openbsc/src/gprs/gprs_utils.c b/openbsc/src/gprs/gprs_utils.c
index 0cf8e4f95..cec07d7ec 100644
--- a/openbsc/src/gprs/gprs_utils.c
+++ b/openbsc/src/gprs/gprs_utils.c
@@ -217,3 +217,120 @@ void gprs_parse_tmsi(const uint8_t *value, uint32_t *tmsi)
 
 	*tmsi = ntohl(tmsi_be);
 }
+
+/* TODO: Move shift functions to libosmocore */
+
+int gprs_shift_v_fixed(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 gprs_match_tv_fixed(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 gprs_match_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;
+
+	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 gprs_shift_lv(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;
+}
+