gprs_gb_parse: Add function to determine TLLI from encoded BSSGP

This will be needed to use the TLLI as link selector parameter
in osmo-gbproxy in an upcoming patch.

Depends: libosmocore.git I397b32a6e6ea3e9d218446138cceafa9b27685dd
Change-Id: Ia6d5300e63ad23987cbdca824db620305bd583d7

2 files changed, 43 insertions, 0 deletions

diff --git a/include/osmocom/sgsn/gprs_gb_parse.h b/include/osmocom/sgsn/gprs_gb_parse.h
index 9f43faed6..58de17f81 100644
--- a/include/osmocom/sgsn/gprs_gb_parse.h
+++ b/include/osmocom/sgsn/gprs_gb_parse.h
@@ -51,6 +51,8 @@ int gprs_gb_parse_llc(uint8_t *llc, size_t llc_len,
 int gprs_gb_parse_bssgp(uint8_t *bssgp, size_t bssgp_len,
 			struct gprs_gb_parse_context *parse_ctx);
 
+int gprs_gb_parse_tlli(const uint8_t *bssgp, size_t bssgp_len, uint32_t *tlli);
+
 const char *gprs_gb_message_name(const struct gprs_gb_parse_context *parse_ctx,
 				 const char *default_msg_name);
 
diff --git a/src/gprs/gprs_gb_parse.c b/src/gprs/gprs_gb_parse.c
index e5de4d4c7..e6a82acb2 100644
--- a/src/gprs/gprs_gb_parse.c
+++ b/src/gprs/gprs_gb_parse.c
@@ -437,6 +437,47 @@ int gprs_gb_parse_llc(uint8_t *llc, size_t llc_len,
 	return gprs_gb_parse_dtap(ghp->data, ghp->data_len, parse_ctx);
 }
 
+/*! Determine the TLLI from the given BSSGP message.
+ *  \param[in] bssgp pointer to start of BSSGP header
+ *  \param[in] bssgp_len length of BSSGP message in octets
+ *  \param[out] tlli TLLI (if any) in host byte order
+ *  \returns 1 if TLLI found; 0 if none found; negative on parse error */
+int gprs_gb_parse_tlli(const uint8_t *bssgp, size_t bssgp_len, uint32_t *tlli)
+{
+	const struct bssgp_normal_hdr *bgph;
+	uint8_t pdu_type;
+
+	if (bssgp_len < sizeof(struct bssgp_normal_hdr))
+		return -EINVAL;
+
+	bgph = (struct bssgp_normal_hdr *)bssgp;
+	pdu_type = bgph->pdu_type;
+
+	if (pdu_type == BSSGP_PDUT_UL_UNITDATA ||
+	    pdu_type == BSSGP_PDUT_DL_UNITDATA) {
+		const struct bssgp_ud_hdr *budh = (struct bssgp_ud_hdr *)bssgp;
+		if (bssgp_len < sizeof(struct bssgp_ud_hdr))
+			return -EINVAL;
+		*tlli = osmo_load32be((const uint8_t *)&budh->tlli);
+		return 1;
+	} else {
+		const uint8_t *data = bgph->data;
+		size_t data_len = bssgp_len - sizeof(*bgph);
+		struct tlv_parsed tp;
+
+		if (bssgp_tlv_parse(&tp, data, data_len) < 0)
+			return -EINVAL;
+
+		if (TLVP_PRESENT(&tp, BSSGP_IE_TLLI)) {
+			*tlli = osmo_load32be(TLVP_VAL(&tp, BSSGP_IE_TLLI));
+			return 1;
+		}
+	}
+
+	/* No TLLI present in message */
+	return 0;
+}
+
 int gprs_gb_parse_bssgp(uint8_t *bssgp, size_t bssgp_len,
 			struct gprs_gb_parse_context *parse_ctx)
 {