gprs: Implement BSSGP MCC/MNC patching

This adds a feature to patch the BSSGP MNC/MCC fields of messages going
to and coming from the SGSN. To enable this feature, the gbproxy's
VTY commands 'core-mobile-country-code' and/or
'core-mobile-network-code' must be used. All packets to the SGSN are
patched to match the configured values. Packets received from the
SGSN are patched to the corresponding values as last seen from the BSS
side.

Note that this will probably not work with a gbproxy used for several
BSS simultaneously.

Note also, that MCC/MNC contained in a LLC IE will not be patched.

Ticket: OW#1185
Sponsored-by: On-Waves ehf
diff --git a/openbsc/include/openbsc/gb_proxy.h b/openbsc/include/openbsc/gb_proxy.h
index 5f1f3a7..e078778 100644
--- a/openbsc/include/openbsc/gb_proxy.h
+++ b/openbsc/include/openbsc/gb_proxy.h
@@ -13,6 +13,10 @@
 
 	/* misc */
 	struct gprs_ns_inst *nsi;
+
+	/* force mcc/mnc */
+	int core_mnc;
+	int core_mcc;
 };
 
 extern struct gbproxy_config gbcfg;
diff --git a/openbsc/src/gprs/Makefile.am b/openbsc/src/gprs/Makefile.am
index 049d41d..87dbc30 100644
--- a/openbsc/src/gprs/Makefile.am
+++ b/openbsc/src/gprs/Makefile.am
@@ -13,7 +13,8 @@
 bin_PROGRAMS = osmo-gbproxy
 endif
 
-osmo_gbproxy_SOURCES = gb_proxy.c gb_proxy_main.c gb_proxy_vty.c
+osmo_gbproxy_SOURCES =  gb_proxy.c gb_proxy_main.c gb_proxy_vty.c \
+			gprs_llc_parse.c crc24.c
 osmo_gbproxy_LDADD = 	$(top_builddir)/src/libcommon/libcommon.a \
 			$(OSMO_LIBS)
 
diff --git a/openbsc/src/gprs/gb_proxy.c b/openbsc/src/gprs/gb_proxy.c
index 9de2ca9..82e9e8c 100644
--- a/openbsc/src/gprs/gb_proxy.c
+++ b/openbsc/src/gprs/gb_proxy.c
@@ -43,6 +43,10 @@
 #include <openbsc/debug.h>
 #include <openbsc/gb_proxy.h>
 
+#include <openbsc/gprs_llc.h>
+#include <openbsc/gsm_04_08.h>
+#include <openbsc/gsm_04_08_gprs.h>
+
 enum gbprox_global_ctr {
 	GBPROX_GLOB_CTR_INV_BVCI,
 	GBPROX_GLOB_CTR_INV_LAI,
@@ -55,6 +59,9 @@
 	GBPROX_GLOB_CTR_RESTART_RESET_SGSN,
 	GBPROX_GLOB_CTR_TX_ERR_SGSN,
 	GBPROX_GLOB_CTR_OTHER_ERR,
+	GBPROX_GLOB_CTR_RAID_PATCHED_BSS,
+	GBPROX_GLOB_CTR_RAID_PATCHED_SGSN,
+	GBPROX_GLOB_CTR_PATCH_ERR,
 };
 
 static const struct rate_ctr_desc global_ctr_description[] = {
@@ -69,6 +76,9 @@
 	{ "restart.sgsn",   "Restarted RESET procedure (SGSN)" },
 	{ "tx-err.sgsn",    "NS Transmission error     (SGSN)" },
 	{ "error",          "Other error                     " },
+	{ "raid-mod.bss",   "RAID patched              (BSS )" },
+	{ "raid-mod.sgsn",  "RAID patched              (SGSN)" },
+	{ "mod-err",        "Patching error                  " },
 };
 
 static const struct rate_ctr_group_desc global_ctrg_desc = {
@@ -112,6 +122,11 @@
 	.ctr_desc = peer_ctr_description,
 };
 
+static struct gbprox_patch_state {
+	int local_mnc;
+	int local_mcc;
+} gbprox_patch_state = {0};
+
 struct gbprox_peer {
 	struct llist_head list;
 
@@ -260,6 +275,118 @@
 	msgb_pull(msg, strip_len);
 }
 
+/* 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)
+{
+	const int old_local_mcc = state->local_mcc;
+	const int old_local_mnc = state->local_mnc;
+	int old_mcc;
+	int old_mnc;
+	struct gprs_ra_id raid;
+
+	gsm48_parse_ra(&raid, raid_enc);
+
+	old_mcc = raid.mcc;
+	old_mnc = raid.mnc;
+
+	if (!to_bss) {
+		/* BSS -> SGSN */
+		/* save BSS side MCC/MNC */
+		if (!gbcfg.core_mcc || raid.mcc == gbcfg.core_mcc) {
+			state->local_mcc = 0;
+		} else {
+			state->local_mcc = raid.mcc;
+			raid.mcc = gbcfg.core_mcc;
+		}
+
+		if (!gbcfg.core_mnc || raid.mnc == gbcfg.core_mnc) {
+			state->local_mnc = 0;
+		} else {
+			state->local_mnc = raid.mnc;
+			raid.mnc = gbcfg.core_mnc;
+		}
+	} else {
+		/* SGSN -> BSS */
+		if (state->local_mcc)
+			raid.mcc = state->local_mcc;
+
+		if (state->local_mnc)
+			raid.mnc = state->local_mnc;
+	}
+
+	if (old_local_mcc != state->local_mcc ||
+	    old_local_mnc != state->local_mnc)
+		LOGP(DGPRS, LOGL_NOTICE,
+		     "Patching RAID %sactivated, msg: %s, "
+		     "local: %d-%d, core: %d-%d, to %s\n",
+		     state->local_mcc || state->local_mnc ?
+		     "" : "de",
+		     log_text,
+		     state->local_mcc, state->local_mnc,
+		     gbcfg.core_mcc, gbcfg.core_mnc,
+		     to_bss ? "BSS" : "SGSN");
+
+	if (state->local_mcc || state->local_mnc) {
+		enum gbprox_global_ctr counter =
+			to_bss ?
+			GBPROX_GLOB_CTR_RAID_PATCHED_SGSN :
+			GBPROX_GLOB_CTR_RAID_PATCHED_BSS;
+
+		LOGP(DGPRS, LOGL_DEBUG,
+		       "Patching %s to %s: "
+		       "%d-%d-%d-%d -> %d-%d-%d-%d\n",
+		       log_text,
+		       to_bss ? "BSS" : "SGSN",
+		       old_mcc, old_mnc, raid.lac, raid.rac,
+		       raid.mcc, raid.mnc, raid.lac, raid.rac);
+
+		gsm48_construct_ra(raid_enc, &raid);
+		rate_ctr_inc(&get_global_ctrg()->ctr[counter]);
+	}
+}
+
+/* patch BSSGP message to use core_mcc/mnc on the SGSN side */
+static void gbprox_patch_bssgp_message(struct msgb *msg, int to_bss)
+{
+	struct bssgp_normal_hdr *bgph;
+	struct bssgp_ud_hdr *budh;
+	struct tlv_parsed tp;
+	uint8_t pdu_type;
+	struct gbprox_patch_state *state = &gbprox_patch_state;
+	uint8_t *data;
+	size_t data_len;
+
+	if (!gbcfg.core_mcc && !gbcfg.core_mnc)
+		return;
+
+	bgph = (struct bssgp_normal_hdr *) msgb_bssgph(msg);
+	budh = (struct bssgp_ud_hdr *) msgb_bssgph(msg);
+	pdu_type = bgph->pdu_type;
+
+	if (to_bss && !state->local_mcc && !state->local_mnc)
+		return;
+
+	if (pdu_type == BSSGP_PDUT_UL_UNITDATA ||
+	    pdu_type == BSSGP_PDUT_DL_UNITDATA) {
+		data = budh->data;
+		data_len = msgb_bssgp_len(msg) - sizeof(*budh);
+	} else {
+		data = bgph->data;
+		data_len = msgb_bssgp_len(msg) - sizeof(*bgph);
+	}
+
+	bssgp_tlv_parse(&tp, data, data_len);
+
+	if (TLVP_PRESENT(&tp, BSSGP_IE_ROUTEING_AREA))
+		gbprox_patch_raid((uint8_t *)TLVP_VAL(&tp, BSSGP_IE_ROUTEING_AREA),
+				  state, to_bss, "ROUTING_AREA");
+
+	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");
+}
+
 /* feed a message down the NS-VC associated with the specified peer */
 static int gbprox_relay2sgsn(struct msgb *old_msg, uint16_t ns_bvci)
 {
@@ -276,6 +403,8 @@
 
 	strip_ns_hdr(msg);
 
+	gbprox_patch_bssgp_message(msg, 0);
+
 	rc = gprs_ns_sendmsg(bssgp_nsi, msg);
 	if (rc < 0)
 		rate_ctr_inc(&get_global_ctrg()->ctr[GBPROX_GLOB_CTR_TX_ERR_SGSN]);
@@ -679,6 +808,9 @@
 	struct gbprox_peer *peer;
 	int remote_end_is_sgsn = nsei == gbcfg.nsip_sgsn_nsei;
 
+	if (remote_end_is_sgsn)
+		gbprox_patch_bssgp_message(msg, 1);
+
 	/* Only BVCI=0 messages need special treatment */
 	if (ns_bvci == 0 || ns_bvci == 1) {
 		if (remote_end_is_sgsn)
diff --git a/openbsc/src/gprs/gb_proxy_main.c b/openbsc/src/gprs/gb_proxy_main.c
index ff2e14b..1f140d4 100644
--- a/openbsc/src/gprs/gb_proxy_main.c
+++ b/openbsc/src/gprs/gb_proxy_main.c
@@ -66,7 +66,7 @@
 	"There is NO WARRANTY, to the extent permitted by law.\r\n";
 
 static char *config_file = "osmo_gbproxy.cfg";
-struct gbproxy_config gbcfg;
+struct gbproxy_config gbcfg = {0};
 static int daemonize = 0;
 
 /* Pointer to the SGSN peer */
diff --git a/openbsc/src/gprs/gb_proxy_vty.c b/openbsc/src/gprs/gb_proxy_vty.c
index 2de0d3b..04431f0 100644
--- a/openbsc/src/gprs/gb_proxy_vty.c
+++ b/openbsc/src/gprs/gb_proxy_vty.c
@@ -51,6 +51,13 @@
 	vty_out(vty, " sgsn nsei %u%s", g_cfg->nsip_sgsn_nsei,
 		VTY_NEWLINE);
 
+	if (g_cfg->core_mcc > 0)
+		vty_out(vty, " core-mobile-country-code %d%s",
+			g_cfg->core_mcc, VTY_NEWLINE);
+	if (g_cfg->core_mnc > 0)
+		vty_out(vty, " core-mobile-network-code %d%s",
+			g_cfg->core_mnc, VTY_NEWLINE);
+
 	return CMD_SUCCESS;
 }
 
@@ -76,6 +83,47 @@
 	return CMD_SUCCESS;
 }
 
+#define GBPROXY_CORE_MNC_STR "Use this network code for the core network\n"
+
+DEFUN(cfg_gbproxy_core_mnc,
+      cfg_gbproxy_core_mnc_cmd,
+      "core-mobile-network-code <1-999>",
+      GBPROXY_CORE_MNC_STR "NCC value\n")
+{
+	g_cfg->core_mnc = atoi(argv[0]);
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_gbproxy_no_core_mnc,
+      cfg_gbproxy_no_core_mnc_cmd,
+      "no core-mobile-network-code",
+      NO_STR GBPROXY_CORE_MNC_STR)
+{
+	g_cfg->core_mnc = 0;
+	return CMD_SUCCESS;
+}
+
+#define GBPROXY_CORE_MCC_STR "Use this country code for the core network\n"
+
+DEFUN(cfg_gbproxy_core_mcc,
+      cfg_gbproxy_core_mcc_cmd,
+      "core-mobile-country-code <1-999>",
+      GBPROXY_CORE_MCC_STR "MCC value\n")
+{
+	g_cfg->core_mcc = atoi(argv[0]);
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_gbproxy_no_core_mcc,
+      cfg_gbproxy_no_core_mcc_cmd,
+      "no core-mobile-country-code",
+      NO_STR GBPROXY_CORE_MCC_STR)
+{
+	g_cfg->core_mcc = 0;
+	return CMD_SUCCESS;
+}
+
+
 int gbproxy_vty_init(void)
 {
 	install_element_ve(&show_gbproxy_cmd);
@@ -87,6 +135,10 @@
 	install_node(&gbproxy_node, config_write_gbproxy);
 	vty_install_default(GBPROXY_NODE);
 	install_element(GBPROXY_NODE, &cfg_nsip_sgsn_nsei_cmd);
+	install_element(GBPROXY_NODE, &cfg_gbproxy_core_mcc_cmd);
+	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);
 
 	return 0;
 }
diff --git a/openbsc/tests/gbproxy/Makefile.am b/openbsc/tests/gbproxy/Makefile.am
index 8e88dc4..9c1b2d0 100644
--- a/openbsc/tests/gbproxy/Makefile.am
+++ b/openbsc/tests/gbproxy/Makefile.am
@@ -9,6 +9,8 @@
 gbproxy_test_SOURCES = gbproxy_test.c
 gbproxy_test_LDADD = \
 			$(top_builddir)/src/gprs/gb_proxy.o \
+			$(top_builddir)/src/gprs/gprs_llc_parse.o \
+			$(top_builddir)/src/gprs/crc24.o \
 			$(top_builddir)/src/libcommon/libcommon.a \
 			$(top_builddir)/src/libbsc/libbsc.a \
 			$(top_builddir)/src/libtrau/libtrau.a \
diff --git a/openbsc/tests/gbproxy/gbproxy_test.c b/openbsc/tests/gbproxy/gbproxy_test.c
index c458733..e93c354 100644
--- a/openbsc/tests/gbproxy/gbproxy_test.c
+++ b/openbsc/tests/gbproxy/gbproxy_test.c
@@ -810,6 +810,8 @@
 	bssgp_nsi = nsi;
 	gbcfg.nsi = bssgp_nsi;
 	gbcfg.nsip_sgsn_nsei = SGSN_NSEI;
+	gbcfg.core_mcc = 123;
+	gbcfg.core_mnc = 456;
 
 	configure_sgsn_peer(&sgsn_peer);
 	configure_bss_peers(bss_peer, ARRAY_SIZE(bss_peer));
diff --git a/openbsc/tests/gbproxy/gbproxy_test.ok b/openbsc/tests/gbproxy/gbproxy_test.ok
index 5f186c2..de19df9 100644
--- a/openbsc/tests/gbproxy/gbproxy_test.ok
+++ b/openbsc/tests/gbproxy/gbproxy_test.ok
@@ -1509,7 +1509,7 @@
 
 NS UNITDATA MESSAGE to SGSN, BVCI 0x0000, msg length 22 (gprs_ns_sendmsg)
 MESSAGE to SGSN at 0x05060708:32000, msg length 26
-00 00 00 00 22 04 82 10 02 07 81 08 08 88 11 22 33 40 50 60 10 00 00 00 00 00 
+00 00 00 00 22 04 82 10 02 07 81 08 08 88 21 63 54 40 50 60 10 00 00 00 00 00 
 
 result (BVC_RESET) = 26
 
@@ -1540,7 +1540,7 @@
 
 NS UNITDATA MESSAGE to SGSN, BVCI 0x0000, msg length 15 (gprs_ns_sendmsg)
 MESSAGE to SGSN at 0x05060708:32000, msg length 19
-00 00 00 00 0b 1f 84 cc d1 75 8b 1b 86 11 22 33 40 50 60 
+00 00 00 00 0b 1f 84 cc d1 75 8b 1b 86 21 63 54 40 50 60 
 
 result (BVC_SUSPEND) = 19
 
@@ -1550,14 +1550,15 @@
 CALLBACK, event 0, msg length 18, bvci 0x0000
 00 00 00 00 0c 1f 84 cc d1 75 8b 1b 86 21 63 54 40 50 60 1d 81 01 
 
-NS UNITDATA MESSAGE to SGSN, BVCI 0x0000, msg length 24 (gprs_ns_sendmsg)
-MESSAGE to SGSN at 0x05060708:32000, msg length 28
-00 00 00 00 41 07 81 05 15 92 0c 1f 84 cc d1 75 8b 1b 86 21 63 54 40 50 60 1d 81 01 
+NS UNITDATA MESSAGE to BSS, BVCI 0x0000, msg length 18 (gprs_ns_sendmsg)
+MESSAGE to BSS at 0x01020304:1111, msg length 22
+00 00 00 00 0c 1f 84 cc d1 75 8b 1b 86 11 22 33 40 50 60 1d 81 01 
 
-result (BVC_SUSPEND_ACK) = 28
+result (BVC_SUSPEND_ACK) = 22
 
 Gbproxy global:
-    Invalid Routing Area Identifier : 1
+    RAID patched              (BSS ): 2
+    RAID patched              (SGSN): 1
 Peers:
   NSEI 4096, BVCI 4098, not blocked, RAI 112-332-16464-96
 --- Send message from BSS 1 to SGSN, BVCI 0x1002 ---
@@ -1570,7 +1571,7 @@
 
 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 11 22 33 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 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 
 
 result (ATTACH REQUEST) = 79
 
@@ -1594,7 +1595,7 @@
 
 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 11 22 33 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 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 
 
 result (UPDATE REQ) = 89
 
@@ -1618,7 +1619,7 @@
 
 NS UNITDATA MESSAGE to SGSN, BVCI 0x1002, msg length 76 (gprs_ns_sendmsg)
 MESSAGE to SGSN at 0x05060708:32000, msg length 80
-00 00 10 02 01 ef e2 b7 00 00 00 04 08 88 11 22 33 40 50 60 75 30 00 80 0e 00 35 01 c0 0d 0a 41 05 03 0c 00 00 1f 10 00 00 00 00 00 00 00 00 02 01 21 28 03 02 61 62 27 14 80 80 21 10 01 00 00 10 81 06 00 00 00 00 83 06 00 00 00 00 5a ff 02 
+00 00 10 02 01 ef e2 b7 00 00 00 04 08 88 21 63 54 40 50 60 75 30 00 80 0e 00 35 01 c0 0d 0a 41 05 03 0c 00 00 1f 10 00 00 00 00 00 00 00 00 02 01 21 28 03 02 61 62 27 14 80 80 21 10 01 00 00 10 81 06 00 00 00 00 83 06 00 00 00 00 5a ff 02 
 
 result (ACT PDP CTX REQ (REPLACE APN)) = 80
 
@@ -1630,12 +1631,13 @@
 
 NS UNITDATA MESSAGE to SGSN, BVCI 0x1002, msg length 76 (gprs_ns_sendmsg)
 MESSAGE to SGSN at 0x05060708:32000, msg length 80
-00 00 10 02 01 ef e2 b7 00 00 00 04 08 88 11 22 33 40 50 60 75 30 00 80 0e 00 35 01 c0 0d 0a 41 05 03 0c 00 00 1f 10 00 00 00 00 00 00 00 00 02 01 21 28 03 02 61 62 27 14 80 80 21 10 01 00 00 10 81 06 00 00 00 00 83 06 00 00 00 00 5a ff 02 
+00 00 10 02 01 ef e2 b7 00 00 00 04 08 88 21 63 54 40 50 60 75 30 00 80 0e 00 35 01 c0 0d 0a 41 05 03 0c 00 00 1f 10 00 00 00 00 00 00 00 00 02 01 21 28 03 02 61 62 27 14 80 80 21 10 01 00 00 10 81 06 00 00 00 00 83 06 00 00 00 00 5a ff 02 
 
 result (ACT PDP CTX REQ (REMOVE APN)) = 80
 
 Gbproxy global:
-    Invalid Routing Area Identifier : 1
+    RAID patched              (BSS ): 6
+    RAID patched              (SGSN): 1
 Peers:
   NSEI 4096, BVCI 4098, not blocked, RAI 112-332-16464-96
 --- Bad cases ---
@@ -1656,13 +1658,15 @@
 
 NS UNITDATA MESSAGE to SGSN, BVCI 0x0000, msg length 24 (gprs_ns_sendmsg)
 MESSAGE to SGSN at 0x05060708:32000, msg length 28
-00 00 00 00 41 07 81 05 15 92 0c 1f 84 cc d1 75 8b 1b 86 00 f1 99 00 63 60 1d 81 01 
+00 00 00 00 41 07 81 05 15 92 0c 1f 84 cc d1 75 8b 1b 86 11 22 33 00 63 60 1d 81 01 
 
 result (BVC_SUSPEND_ACK) = 28
 
 Gbproxy global:
     Invalid BVC Identifier          : 1
-    Invalid Routing Area Identifier : 2
+    Invalid Routing Area Identifier : 1
+    RAID patched              (BSS ): 6
+    RAID patched              (SGSN): 2
 Peers:
   NSEI 4096, BVCI 4098, not blocked, RAI 112-332-16464-96
 ===== GbProxy test END