gprs: Add MCC/MNC patch support for LLC/GMM messages

This patch extends the BSSGP patch code to also patch LLC information
elements along with MCC/MNC patching support for the following messages:

- Attach Request
- Attach Accept
- Routing Area Update Request
- Routing Area Update Accept
- P-TMSI reallocation command

Note that encrypted packets will not be patched.

Ticket: OW#1185
Sponsored-by: On-Waves ehf

4 files changed, 345 insertions, 9 deletions

diff --git a/openbsc/include/openbsc/gb_proxy.h b/openbsc/include/openbsc/gb_proxy.h
index e078778e3..18ed81f37 100644
--- a/openbsc/include/openbsc/gb_proxy.h
+++ b/openbsc/include/openbsc/gb_proxy.h
@@ -7,6 +7,15 @@
 #include <osmocom/gprs/gprs_ns.h>
 #include <osmocom/vty/command.h>
 
+enum gbproxy_patch_mode {
+	GBPROX_PATCH_DEFAULT,
+	GBPROX_PATCH_BSSGP,		/*!< BSGGP messages only */
+	GBPROX_PATCH_LLC_ATTACH_REQ,	/*!< BSSGP and Attach Request */
+	GBPROX_PATCH_LLC_ATTACH,	/*!< BSSGP and Attach Request/Response */
+	GBPROX_PATCH_LLC_GMM,		/*!< BSSGP and all GMM msgs */
+	GBPROX_PATCH_LLC,		/*!< BSSGP and all supported LLC msgs */
+};
+
 struct gbproxy_config {
 	/* parsed from config file */
 	uint16_t nsip_sgsn_nsei;
@@ -17,6 +26,7 @@ struct gbproxy_config {
 	/* force mcc/mnc */
 	int core_mnc;
 	int core_mcc;
+	enum gbproxy_patch_mode patch_mode;
 };
 
 extern struct gbproxy_config gbcfg;
diff --git a/openbsc/src/gprs/gb_proxy.c b/openbsc/src/gprs/gb_proxy.c
index 82e9e8c70..6daf45077 100644
--- a/openbsc/src/gprs/gb_proxy.c
+++ b/openbsc/src/gprs/gb_proxy.c
@@ -61,6 +61,7 @@ enum gbprox_global_ctr {
 	GBPROX_GLOB_CTR_OTHER_ERR,
 	GBPROX_GLOB_CTR_RAID_PATCHED_BSS,
 	GBPROX_GLOB_CTR_RAID_PATCHED_SGSN,
+	GBPROX_GLOB_CTR_PATCH_CRYPT_ERR,
 	GBPROX_GLOB_CTR_PATCH_ERR,
 };
 
@@ -78,7 +79,8 @@ static const struct rate_ctr_desc global_ctr_description[] = {
 	{ "error",          "Other error                     " },
 	{ "raid-mod.bss",   "RAID patched              (BSS )" },
 	{ "raid-mod.sgsn",  "RAID patched              (SGSN)" },
-	{ "mod-err",        "Patching error                  " },
+	{ "mod-crypt-err",  "Patch error: encrypted          " },
+	{ "mod-err",        "Patch error: other              " },
 };
 
 static const struct rate_ctr_group_desc global_ctrg_desc = {
@@ -275,6 +277,22 @@ static void strip_ns_hdr(struct msgb *msg)
 	msgb_pull(msg, strip_len);
 }
 
+/* check whether patching is enabled at this level */
+static int patching_is_enabled(enum gbproxy_patch_mode need_at_least)
+{
+	enum gbproxy_patch_mode patch_mode = gbcfg.patch_mode;
+	if (patch_mode == GBPROX_PATCH_DEFAULT)
+		patch_mode = GBPROX_PATCH_LLC;
+
+	return need_at_least <= patch_mode;
+}
+
+/* check whether patching is enabled at this level */
+static int patching_is_required(enum gbproxy_patch_mode need_at_least)
+{
+	return need_at_least <= gbcfg.patch_mode;
+}
+
 /* patch RA identifier in place, update peer accordingly */
 static void gbprox_patch_raid(uint8_t *raid_enc, struct gbprox_patch_state *state,
 			      int to_bss, const char *log_text)
@@ -346,6 +364,271 @@ static void gbprox_patch_raid(uint8_t *raid_enc, struct gbprox_patch_state *stat
 	}
 }
 
+static int gbprox_patch_gmm_attach_req(struct msgb *msg,
+				       uint8_t *data, size_t data_len,
+				       struct gbprox_patch_state *state,
+				       int to_bss, int *len_change)
+{
+	/* Check minimum length, always includes the RAI */
+	if (data_len < 23)
+		return 0;
+
+	/* Skip MS network capability */
+	if (data[0] < 1 || data[0] > 2)
+		/* invalid */
+		return 0;
+	data_len -= data[0] + 1;
+	data += data[0] + 1;
+
+	/* Skip Attach type */
+	/* Skip Ciphering key sequence number */
+	/* Skip DRX parameter */
+	data_len -= 3;
+	data += 3;
+
+	/* Skip Mobile identity */
+	if (data[0] < 5 || data[0] > 8)
+		/* invalid */
+		return 0;
+	data_len -= data[0] + 1;
+	data += data[0] + 1;
+
+	gbprox_patch_raid(data, state, to_bss, "LLC/ATTACH_REQ");
+
+	return 1;
+}
+
+static int gbprox_patch_gmm_attach_ack(struct msgb *msg,
+				       uint8_t *data, size_t data_len,
+				       struct gbprox_patch_state *state,
+				       int to_bss, int *len_change)
+{
+	/* Check minimum length, always includes the RAI */
+	if (data_len < 9)
+		return 0;
+
+	/* Skip Attach result */
+	/* Skip Force to standby */
+	/* Skip Periodic RA update timer */
+	/* Skip Radio priority for SMS */
+	/* Skip Spare half octet */
+	data_len -= 3;
+	data += 3;
+
+	gbprox_patch_raid(data, state, to_bss, "LLC/ATTACH_ACK");
+
+	return 1;
+}
+
+static int gbprox_patch_gmm_ra_upd_req(struct msgb *msg,
+				       uint8_t *data, size_t data_len,
+				       struct gbprox_patch_state *state,
+				       int to_bss, int *len_change)
+{
+	/* Check minimum length, always includes the RAI */
+	if (data_len < 13)
+		return 0;
+
+	/* Skip Update type */
+	/* Skip GPRS ciphering key sequence number */
+	data_len -= 1;
+	data += 1;
+
+	gbprox_patch_raid(data, state, to_bss, "LLC/RA_UPD_REQ");
+
+	return 1;
+}
+
+static int gbprox_patch_gmm_ra_upd_ack(struct msgb *msg,
+				       uint8_t *data, size_t data_len,
+				       struct gbprox_patch_state *state,
+				       int to_bss, int *len_change)
+{
+	/* Check minimum length, always includes the RAI */
+	if (data_len < 8)
+		return 0;
+
+	/* Skip Force to standby */
+	/* Skip Update result */
+	/* Skip Periodic RA update timer */
+	data_len -= 2;
+	data += 2;
+
+	gbprox_patch_raid(data, state, to_bss, "LLC/RA_UPD_ACK");
+
+	return 1;
+}
+
+static int gbprox_patch_gmm_ptmsi_reall_cmd(struct msgb *msg,
+					    uint8_t *data, size_t data_len,
+					    struct gbprox_patch_state *state,
+					    int to_bss, int *len_change)
+{
+	/* Check minimum length, always includes the RAI */
+	if (data_len < 12)
+		return 0;
+
+	/* Skip Allocated P-TMSI */
+	if (data[0] != 5)
+		/* invalid */
+		return 0;
+	data_len -= 6;
+	data += 6;
+
+	gbprox_patch_raid(data, state, to_bss, "LLC/PTMSI_REALL_CMD");
+
+	return 1;
+}
+
+static int gbprox_patch_dtap(struct msgb *msg, uint8_t *data, size_t data_len,
+			     struct gbprox_patch_state *state, int to_bss,
+			     int *len_change)
+{
+	struct gsm48_hdr *g48h;
+
+	*len_change = 0;
+
+	if (data_len < 2)
+		return 0;
+
+	g48h = (struct gsm48_hdr *)data;
+
+	data += sizeof(struct gsm48_hdr);
+	data_len -= sizeof(struct gsm48_hdr);
+
+	if ((g48h->proto_discr & 0x0f) != GSM48_PDISC_MM_GPRS &&
+	    (g48h->proto_discr & 0x0f) != GSM48_PDISC_SM_GPRS)
+		return 0;
+
+	switch (g48h->msg_type) {
+	case GSM48_MT_GMM_ATTACH_REQ:
+		return gbprox_patch_gmm_attach_req(msg, data, data_len,
+						   state, to_bss, len_change);
+
+	case GSM48_MT_GMM_ATTACH_ACK:
+		if (!patching_is_enabled(GBPROX_PATCH_LLC_ATTACH))
+			break;
+		return gbprox_patch_gmm_attach_ack(msg, data, data_len,
+						   state, to_bss, len_change);
+
+	case GSM48_MT_GMM_RA_UPD_REQ:
+		if (!patching_is_enabled(GBPROX_PATCH_LLC_GMM))
+			break;
+		return gbprox_patch_gmm_ra_upd_req(msg, data, data_len,
+						   state, to_bss, len_change);
+
+	case GSM48_MT_GMM_RA_UPD_ACK:
+		if (!patching_is_enabled(GBPROX_PATCH_LLC_GMM))
+			break;
+		return gbprox_patch_gmm_ra_upd_ack(msg, data, data_len,
+						   state, to_bss, len_change);
+
+	case GSM48_MT_GMM_PTMSI_REALL_CMD:
+		if (!patching_is_enabled(GBPROX_PATCH_LLC_GMM))
+			break;
+		return gbprox_patch_gmm_ptmsi_reall_cmd(msg, data, data_len,
+							state, to_bss, len_change);
+
+	default:
+		break;
+	};
+
+	return 0;
+}
+
+static void gbprox_patch_llc(struct msgb *msg, uint8_t *llc, size_t llc_len,
+			     struct gbprox_patch_state *state, int to_bss)
+{
+	struct gprs_llc_hdr_parsed ghp = {0};
+	int rc;
+	uint8_t *data;
+	size_t data_len;
+	int fcs;
+	int len_change = 0;
+	const char *err_info = NULL;
+	int err_ctr = -1;
+
+	/* parse LLC */
+	rc = gprs_llc_hdr_parse(&ghp, llc, llc_len);
+	gprs_llc_hdr_dump(&ghp);
+	if (rc != 0) {
+		LOGP(DLLC, LOGL_NOTICE, "Error during LLC header parsing\n");
+		return;
+	}
+
+	fcs = gprs_llc_fcs(llc, ghp.crc_length);
+	LOGP(DLLC, LOGL_DEBUG, "Got LLC message, CRC: %06x (computed %06x)\n",
+	     ghp.fcs, fcs);
+
+	if (!ghp.data)
+		return;
+
+	if (ghp.sapi != GPRS_SAPI_GMM)
+		return;
+
+	if (ghp.cmd != GPRS_LLC_UI)
+		return;
+
+	if (ghp.is_encrypted) {
+		if (patching_is_required(GBPROX_PATCH_LLC_ATTACH)) {
+			/* Patching LLC messages has been requested explicitly,
+			 * but the message (including the type) is encrypted,
+			 * so we possibly fail to patch the LLC part of the
+			 * message. */
+
+			err_info = "GMM message is encrypted";
+			err_ctr = GBPROX_GLOB_CTR_PATCH_CRYPT_ERR;
+			goto patch_error;
+		}
+
+		return;
+	}
+
+	/* fix DTAP GMM/GSM */
+	data = ghp.data;
+	data_len = ghp.data_len;
+
+	rc = gbprox_patch_dtap(msg, data, data_len, state, to_bss, &len_change);
+
+	if (rc > 0) {
+		llc_len += len_change;
+		ghp.crc_length += len_change;
+
+		/* Fix LLC IE len */
+		if (llc[-2] == BSSGP_IE_LLC_PDU && llc[-1] & 0x80) {
+			/* most probably a one byte length */
+			if (llc_len > 127) {
+				err_info = "Cannot increase size";
+				err_ctr = GBPROX_GLOB_CTR_PATCH_ERR;
+				goto patch_error;
+			}
+			llc[-1] = llc_len | 0x80;
+		} else {
+			llc[-2] = (llc_len >> 8) & 0x7f;
+			llc[-1] = llc_len & 0xff;
+		}
+
+		/* Fix FCS */
+		fcs = gprs_llc_fcs(llc, ghp.crc_length);
+		LOGP(DLLC, LOGL_DEBUG, "Updated LLC message, CRC: %06x -> %06x\n",
+		     ghp.fcs, fcs);
+
+		llc[llc_len - 3] = fcs & 0xff;
+		llc[llc_len - 2] = (fcs >> 8) & 0xff;
+		llc[llc_len - 1] = (fcs >> 16) & 0xff;
+	}
+
+	return;
+
+patch_error:
+	OSMO_ASSERT(err_ctr >= 0);
+	rate_ctr_inc(&get_global_ctrg()->ctr[err_ctr]);
+	LOGP(DGPRS, LOGL_ERROR,
+	     "Failed to patch BSSGP/GMM message as requested: %s.\n", err_info);
+
+	return;
+}
+
 /* patch BSSGP message to use core_mcc/mnc on the SGSN side */
 static void gbprox_patch_bssgp_message(struct msgb *msg, int to_bss)
 {
@@ -385,6 +668,15 @@ static void gbprox_patch_bssgp_message(struct msgb *msg, int to_bss)
 	if (TLVP_PRESENT(&tp, BSSGP_IE_CELL_ID))
 		gbprox_patch_raid((uint8_t *)TLVP_VAL(&tp, BSSGP_IE_CELL_ID),
 				  state, to_bss, "CELL_ID");
+
+	if (TLVP_PRESENT(&tp, BSSGP_IE_LLC_PDU) &&
+	    patching_is_enabled(GBPROX_PATCH_LLC_ATTACH_REQ)) {
+		uint8_t *llc = (uint8_t *)TLVP_VAL(&tp, BSSGP_IE_LLC_PDU);
+		size_t llc_len = TLVP_LEN(&tp, BSSGP_IE_LLC_PDU);
+		gbprox_patch_llc(msg, llc, llc_len, state, to_bss);
+		/* Note that the tp struct might contain invalid pointers here
+		 * if the LLC field has changed its size */
+	}
 }
 
 /* feed a message down the NS-VC associated with the specified peer */
diff --git a/openbsc/src/gprs/gb_proxy_vty.c b/openbsc/src/gprs/gb_proxy_vty.c
index 04431f0e3..48033473f 100644
--- a/openbsc/src/gprs/gb_proxy_vty.c
+++ b/openbsc/src/gprs/gb_proxy_vty.c
@@ -44,6 +44,16 @@ static struct cmd_node gbproxy_node = {
 	1,
 };
 
+static const struct value_string patch_modes[] = {
+	{GBPROX_PATCH_DEFAULT, "default"},
+	{GBPROX_PATCH_BSSGP, "bssgp"},
+	{GBPROX_PATCH_LLC_ATTACH_REQ, "llc-attach-req"},
+	{GBPROX_PATCH_LLC_ATTACH, "llc-attach"},
+	{GBPROX_PATCH_LLC_GMM, "llc-gmm"},
+	{GBPROX_PATCH_LLC, "llc"},
+	{0, NULL}
+};
+
 static int config_write_gbproxy(struct vty *vty)
 {
 	vty_out(vty, "gbproxy%s", VTY_NEWLINE);
@@ -58,6 +68,11 @@ static int config_write_gbproxy(struct vty *vty)
 		vty_out(vty, " core-mobile-network-code %d%s",
 			g_cfg->core_mnc, VTY_NEWLINE);
 
+	if (g_cfg->patch_mode != GBPROX_PATCH_DEFAULT)
+		vty_out(vty, " patch-mode %s%s",
+			get_value_string(patch_modes, g_cfg->patch_mode),
+			VTY_NEWLINE);
+
 	return CMD_SUCCESS;
 }
 
@@ -123,6 +138,24 @@ DEFUN(cfg_gbproxy_no_core_mcc,
 	return CMD_SUCCESS;
 }
 
+DEFUN(cfg_gbproxy_patch_mode,
+      cfg_gbproxy_patch_mode_cmd,
+      "patch-mode (default|bssgp|llc-attach-req|llc-attach|llc)",
+      "Set patch mode\n"
+      "Use build-in default (at least llc-attach-req)\n"
+      "Only patch BSSGP headers\n"
+      "Patch BSSGP headers and LLC Attach Request messages\n"
+      "Patch BSSGP headers and LLC Attach Request/Accept messages\n"
+      "Patch BSSGP headers and all supported GMM LLC messages\n"
+      )
+{
+	int val = get_string_value(patch_modes, argv[0]);
+	OSMO_ASSERT(val >= 0);
+	g_cfg->patch_mode = val;
+	return CMD_SUCCESS;
+}
+
+
 
 int gbproxy_vty_init(void)
 {
@@ -139,6 +172,7 @@ int gbproxy_vty_init(void)
 	install_element(GBPROXY_NODE, &cfg_gbproxy_core_mnc_cmd);
 	install_element(GBPROXY_NODE, &cfg_gbproxy_no_core_mcc_cmd);
 	install_element(GBPROXY_NODE, &cfg_gbproxy_no_core_mnc_cmd);
+	install_element(GBPROXY_NODE, &cfg_gbproxy_patch_mode_cmd);
 
 	return 0;
 }
diff --git a/openbsc/tests/gbproxy/gbproxy_test.ok b/openbsc/tests/gbproxy/gbproxy_test.ok
index de19df991..64ed996dc 100644
--- a/openbsc/tests/gbproxy/gbproxy_test.ok
+++ b/openbsc/tests/gbproxy/gbproxy_test.ok
@@ -1571,7 +1571,7 @@ CALLBACK, event 0, msg length 75, bvci 0x1002
 
 NS UNITDATA MESSAGE to SGSN, BVCI 0x1002, msg length 75 (gprs_ns_sendmsg)
 MESSAGE to SGSN at 0x05060708:32000, msg length 79
-00 00 10 02 01 bb c5 46 79 00 00 04 08 88 21 63 54 40 50 60 75 30 00 80 0e 00 34 01 c0 01 08 01 02 f5 e0 21 08 02 05 f4 fb c5 46 79 11 22 33 40 50 60 19 18 b3 43 2b 25 96 62 00 60 80 9a c2 c6 62 00 60 80 ba c8 c6 62 00 60 80 00 16 6d 01 
+00 00 10 02 01 bb c5 46 79 00 00 04 08 88 21 63 54 40 50 60 75 30 00 80 0e 00 34 01 c0 01 08 01 02 f5 e0 21 08 02 05 f4 fb c5 46 79 21 63 54 40 50 60 19 18 b3 43 2b 25 96 62 00 60 80 9a c2 c6 62 00 60 80 ba c8 c6 62 00 60 80 00 8e cd 32 
 
 result (ATTACH REQUEST) = 79
 
@@ -1583,7 +1583,7 @@ CALLBACK, event 0, msg length 88, bvci 0x1002
 
 NS UNITDATA MESSAGE to BSS, BVCI 0x1002, msg length 88 (gprs_ns_sendmsg)
 MESSAGE to BSS at 0x01020304:1111, msg length 92
-00 00 10 02 00 bb c5 46 79 00 50 20 16 82 02 58 13 99 18 b3 43 2b 25 96 62 00 60 80 9a c2 c6 62 00 60 80 ba c8 c6 62 00 60 80 00 0a 82 08 02 0d 88 11 12 13 14 15 16 17 18 00 81 00 0e 9e 41 c0 05 08 02 01 49 04 21 63 54 40 50 60 19 cd d7 08 17 16 18 05 f4 fb c5 47 22 42 67 9a 
+00 00 10 02 00 bb c5 46 79 00 50 20 16 82 02 58 13 99 18 b3 43 2b 25 96 62 00 60 80 9a c2 c6 62 00 60 80 ba c8 c6 62 00 60 80 00 0a 82 08 02 0d 88 11 12 13 14 15 16 17 18 00 81 00 0e 9e 41 c0 05 08 02 01 49 04 11 22 33 40 50 60 19 cd d7 08 17 16 18 05 f4 fb c5 47 22 af 3d ab 
 
 result (ATTACH ACCEPT) = 92
 
@@ -1595,7 +1595,7 @@ CALLBACK, event 0, msg length 85, bvci 0x1002
 
 NS UNITDATA MESSAGE to SGSN, BVCI 0x1002, msg length 85 (gprs_ns_sendmsg)
 MESSAGE to SGSN at 0x05060708:32000, msg length 89
-00 00 10 02 01 af e2 80 6e 00 00 04 08 88 21 63 54 40 50 60 70 80 00 80 0e 00 3e 01 c0 15 08 08 10 11 22 33 40 50 60 1d 19 13 42 33 57 2b f7 c8 48 02 13 48 50 c8 48 02 14 48 50 c8 48 02 17 49 10 c8 48 02 00 19 8b b2 92 17 16 27 07 04 31 02 e5 e0 32 02 20 00 96 3e 97 
+00 00 10 02 01 af e2 80 6e 00 00 04 08 88 21 63 54 40 50 60 70 80 00 80 0e 00 3e 01 c0 15 08 08 10 21 63 54 40 50 60 1d 19 13 42 33 57 2b f7 c8 48 02 13 48 50 c8 48 02 14 48 50 c8 48 02 17 49 10 c8 48 02 00 19 8b b2 92 17 16 27 07 04 31 02 e5 e0 32 02 20 00 1d f0 41 
 
 result (UPDATE REQ) = 89
 
@@ -1607,7 +1607,7 @@ CALLBACK, event 0, msg length 91, bvci 0x1002
 
 NS UNITDATA MESSAGE to BSS, BVCI 0x1002, msg length 91 (gprs_ns_sendmsg)
 MESSAGE to BSS at 0x01020304:1111, msg length 95
-00 00 10 02 00 af e2 80 6e 00 50 20 16 82 02 58 13 9d 19 13 42 33 57 2b f7 c8 48 02 13 48 50 c8 48 02 14 48 50 c8 48 02 17 49 10 c8 48 02 00 0a 82 07 04 0d 88 11 12 13 14 15 16 17 18 00 81 00 0e 9d 41 c0 19 08 09 00 49 21 63 54 40 50 60 19 54 ab b3 18 05 f4 ef e2 81 17 17 16 c3 bf cc 
+00 00 10 02 00 af e2 80 6e 00 50 20 16 82 02 58 13 9d 19 13 42 33 57 2b f7 c8 48 02 13 48 50 c8 48 02 14 48 50 c8 48 02 17 49 10 c8 48 02 00 0a 82 07 04 0d 88 11 12 13 14 15 16 17 18 00 81 00 0e 9d 41 c0 19 08 09 00 49 11 22 33 40 50 60 19 54 ab b3 18 05 f4 ef e2 81 17 17 16 2e e5 fd 
 
 result (RA UPD ACC) = 95
 
@@ -1636,8 +1636,8 @@ MESSAGE to SGSN at 0x05060708:32000, msg length 80
 result (ACT PDP CTX REQ (REMOVE APN)) = 80
 
 Gbproxy global:
-    RAID patched              (BSS ): 6
-    RAID patched              (SGSN): 1
+    RAID patched              (BSS ): 8
+    RAID patched              (SGSN): 3
 Peers:
   NSEI 4096, BVCI 4098, not blocked, RAI 112-332-16464-96
 --- Bad cases ---
@@ -1665,8 +1665,8 @@ result (BVC_SUSPEND_ACK) = 28
 Gbproxy global:
     Invalid BVC Identifier          : 1
     Invalid Routing Area Identifier : 1
-    RAID patched              (BSS ): 6
-    RAID patched              (SGSN): 2
+    RAID patched              (BSS ): 8
+    RAID patched              (SGSN): 4
 Peers:
   NSEI 4096, BVCI 4098, not blocked, RAI 112-332-16464-96
 ===== GbProxy test END