gsm48: Add a generic MI from classmark+mi extraction.

This is a generic MI extraction for the MI if it is followed
after a classmark. For the Phase1 Phones the classmark2 is not
four bytes but it might be different. This code can be used
by the CM Service Request handling as well.

2 files changed, 14 insertions, 6 deletions

diff --git a/openbsc/include/openbsc/gsm_04_08.h b/openbsc/include/openbsc/gsm_04_08.h
index a49f91ed6..142e2453b 100644
--- a/openbsc/include/openbsc/gsm_04_08.h
+++ b/openbsc/include/openbsc/gsm_04_08.h
@@ -52,6 +52,7 @@ int decode_bcd_number(char *output, int output_len, const u_int8_t *bcd_lv,
 		      int h_len);
 
 int send_siemens_mrpci(struct gsm_lchan *lchan, u_int8_t *classmark2_lv);
+int gsm48_extract_mi(uint8_t *classmark2, int length, char *mi_string, uint8_t *mi_type);
 int gsm48_paging_extract_mi(struct gsm48_pag_resp *pag, int length, char *mi_string, u_int8_t *mi_type);
 int gsm48_handle_paging_resp(struct msgb *msg, struct gsm_subscriber *subscr);
 
diff --git a/openbsc/src/gsm_04_08_utils.c b/openbsc/src/gsm_04_08_utils.c
index db8c3a5e4..d7e42ddf7 100644
--- a/openbsc/src/gsm_04_08_utils.c
+++ b/openbsc/src/gsm_04_08_utils.c
@@ -243,23 +243,30 @@ int send_siemens_mrpci(struct gsm_lchan *lchan,
 	return rsl_siemens_mrpci(lchan, &mrpci);
 }
 
-int gsm48_paging_extract_mi(struct gsm48_pag_resp *resp, int length,
-			    char *mi_string, u_int8_t *mi_type)
+int gsm48_extract_mi(uint8_t *classmark2_lv, int length, char *mi_string, uint8_t *mi_type)
 {
-	u_int8_t *classmark2_lv = (uint8_t *) &resp->classmark2;
-
 	/* Check the size for the classmark */
-	if (length < 2 + *classmark2_lv)
+	if (length < 1 + *classmark2_lv)
 		return -1;
 
 	u_int8_t *mi_lv = classmark2_lv + *classmark2_lv + 1;
-	if (length < 3 + *classmark2_lv + mi_lv[0])
+	if (length < 2 + *classmark2_lv + mi_lv[0])
 		return -2;
 
 	*mi_type = mi_lv[1] & GSM_MI_TYPE_MASK;
 	return gsm48_mi_to_string(mi_string, GSM48_MI_SIZE, mi_lv+1, *mi_lv);
 }
 
+int gsm48_paging_extract_mi(struct gsm48_pag_resp *resp, int length,
+			    char *mi_string, u_int8_t *mi_type)
+{
+	static const uint32_t classmark_offset =
+		offsetof(struct gsm48_pag_resp, classmark2);
+	u_int8_t *classmark2_lv = (uint8_t *) &resp->classmark2;
+	return gsm48_extract_mi(classmark2_lv, length - classmark_offset,
+				mi_string, mi_type);
+}
+
 int gsm48_handle_paging_resp(struct msgb *msg, struct gsm_subscriber *subscr)
 {
 	struct gsm_bts *bts = msg->lchan->ts->trx->bts;