sysinfo.c: Fix SI4 GPRS patching which overwrote CBCH IE

In Change-Id I1fd513ea03297918d15d4b28ed454f9b6dd6ebfa we introduced
patching of SI4 to indicate GPRS presence in terms of PCU connection
status.  Unfortauntely this didn't account for optional IEs being
present in SI4, and hence overwrote any CBCH related information
elements, if present.

This in turn meant that since the above-mentioned commit, you could
have either a GPRS-capable, network, or a Cell Broadcast capable one.

Change-Id: I0ee0cf736e2fb74a6759a68101f699b4ec2ef54e
Related: OS#3075

2 files changed, 42 insertions, 9 deletions

diff --git a/src/common/rsl.c b/src/common/rsl.c
index ef23039b..8e27e777 100644
--- a/src/common/rsl.c
+++ b/src/common/rsl.c
@@ -380,13 +380,16 @@ static int rsl_rx_bcch_info(struct gsm_bts_trx *trx, struct msgb *msg)
 			regenerate_si3_restoctets(bts);
 		} else if (SYSINFO_TYPE_4 == osmo_si) {
 			/* decode original SI4 Rest Octets as sent by BSC */
-			const uint8_t *si4_ro_buf = (uint8_t *) GSM_BTS_SI(bts, osmo_si);
-			si4_ro_buf += offsetof(struct gsm48_system_information_type_4, data);
-			osmo_gsm48_rest_octets_si4_decode(&bts->si4_ro_decoded, si4_ro_buf,
-							  GSM_MACBLOCK_LEN - offsetof(struct gsm48_system_information_type_4, data));
-			/* patch out GPRS indicator from binary if PCU is not connected; will be enabled
-			 * after PCU connects */
-			regenerate_si4_restoctets(bts);
+			const uint8_t *si4 = (uint8_t *) GSM_BTS_SI(bts, osmo_si);
+			int si4_ro_offset = get_si4_ro_offset(si4);
+			if (si4_ro_offset < GSM_MACBLOCK_LEN) {
+				osmo_gsm48_rest_octets_si4_decode(&bts->si4_ro_decoded,
+								  si4 + si4_ro_offset,
+								  GSM_MACBLOCK_LEN - si4_ro_offset);
+				/* patch out GPRS indicator from binary if PCU is not connected; will be
+				 * enabled after PCU connects */
+				regenerate_si4_restoctets(bts);
+			}
 		}
 
 		if (SYSINFO_TYPE_13 == osmo_si)
diff --git a/src/common/sysinfo.c b/src/common/sysinfo.c
index f3deb0dc..9eea5396 100644
--- a/src/common/sysinfo.c
+++ b/src/common/sysinfo.c
@@ -1,4 +1,4 @@
-/* (C) 2011-2019 by Harald Welte <laforge@gnumonks.org>
+/* (C) 2011-2020 by Harald Welte <laforge@gnumonks.org>
  *
  * All Rights Reserved
  *
@@ -18,6 +18,7 @@
  */
 
 #include <stdint.h>
+#include <errno.h>
 
 #include <osmocom/gsm/gsm_utils.h>
 #include <osmocom/gsm/sysinfo.h>
@@ -212,11 +213,34 @@ void regenerate_si3_restoctets(struct gsm_bts *bts)
 	osmo_gsm48_rest_octets_si3_encode(si3_buf + si3_size, &si3ro_tmp);
 }
 
+/* get the offset of the SI4 rest octets */
+int get_si4_ro_offset(const uint8_t *si4_buf)
+{
+	const struct gsm48_system_information_type_4 *si4 =
+					(const struct gsm48_system_information_type_4 *) si4_buf;
+	int si4_size;
+
+	/* start with the length of the mandatory part */
+	si4_size = offsetof(struct gsm48_system_information_type_4, data);
+	/* then add optional parts, if any */
+	if (si4->data[0] == GSM48_IE_CBCH_CHAN_DESC) {
+		/* fixed 4-byte TV IE, see Table 9.1.36.1 of TS 44.018 */
+		si4_size += 4;
+		if (si4->data[4] == GSM48_IE_CBCH_MOB_AL)
+			si4_size += TLV_GROSS_LEN(si4->data[5]);
+	}
+
+	if (si4_size >= GSM_MACBLOCK_LEN)
+		return -EINVAL;
+
+	return si4_size;
+}
+
 /* re-generate SI4 restoctets with GPRS indicator depending on the PCU socket connection state */
 void regenerate_si4_restoctets(struct gsm_bts *bts)
 {
 	uint8_t *si4_buf = GSM_BTS_SI(bts, SYSINFO_TYPE_4);
-	size_t si4_size = offsetof(struct gsm48_system_information_type_4, data);
+	size_t si4_size;
 	struct osmo_gsm48_si_ro_info si4ro_tmp;
 
 	/* If BSC has never set SI4, there's nothing to patch */
@@ -227,6 +251,12 @@ void regenerate_si4_restoctets(struct gsm_bts *bts)
 	if (!bts->si4_ro_decoded.gprs_ind.present)
 		return;
 
+	si4_size = get_si4_ro_offset(si4_buf);
+	if (si4_size < 0) {
+		LOGP(DPCU, LOGL_ERROR, "Cannot parse SI4, hence not patching GPRS indicator\n");
+		return;
+	}
+
 	/* Create a temporary copy and patch that, if no PCU is around */
 	si4ro_tmp = bts->si4_ro_decoded;
 	if (!pcu_connected()) {