nat: Patch the destination SMS address of a message

Use the same filtering infrasturcture to patch the SMSC
address in a CP-DATA/RP-DATA message. Add a very simple
testcase for this code.

2 files changed, 135 insertions, 6 deletions

diff --git a/openbsc/src/osmo-bsc_nat/bsc_nat_utils.c b/openbsc/src/osmo-bsc_nat/bsc_nat_utils.c
index 293271204..ae9891e04 100644
--- a/openbsc/src/osmo-bsc_nat/bsc_nat_utils.c
+++ b/openbsc/src/osmo-bsc_nat/bsc_nat_utils.c
@@ -34,6 +34,7 @@
 #include <osmocom/gsm/gsm0808.h>
 
 #include <osmocom/gsm/protocol/gsm_08_08.h>
+#include <osmocom/gsm/protocol/gsm_04_11.h>
 
 #include <osmocom/sccp/sccp.h>
 
@@ -95,6 +96,7 @@ struct bsc_nat *bsc_nat_alloc(void)
 	INIT_LLIST_HEAD(&nat->access_lists);
 	INIT_LLIST_HEAD(&nat->dests);
 	INIT_LLIST_HEAD(&nat->num_rewr);
+	INIT_LLIST_HEAD(&nat->smsc_rewr);
 
 	nat->stats.sccp.conn = osmo_counter_alloc("nat.sccp.conn");
 	nat->stats.sccp.calls = osmo_counter_alloc("nat.sccp.calls");
@@ -919,6 +921,130 @@ static struct msgb *rewrite_setup(struct bsc_nat *nat, struct msgb *msg,
 	return out;
 }
 
+static struct msgb *rewrite_smsc(struct bsc_nat *nat, struct msgb *msg,
+				 struct bsc_nat_parsed *parsed, const char *imsi,
+				 struct gsm48_hdr *hdr48, const uint32_t len)
+{
+	unsigned int payload_len;
+	unsigned int cp_len;
+
+	uint8_t ref;
+	uint8_t orig_addr_len, *orig_addr_ptr;
+	uint8_t dest_addr_len, *dest_addr_ptr;
+	uint8_t data_len, *data_ptr;
+	char smsc_addr[30];
+	uint8_t new_addr[12];
+
+	struct bsc_nat_num_rewr_entry *entry;
+	char *new_number = NULL;
+	uint8_t new_addr_len;
+	struct gsm48_hdr *new_hdr48;
+	struct msgb *out;
+
+	payload_len = len - sizeof(*hdr48);
+	if (payload_len < 1) {
+		LOGP(DNAT, LOGL_ERROR, "SMS too short for things. %d\n", payload_len);
+		return NULL;
+	}
+
+	cp_len = hdr48->data[0];
+	if (payload_len + 1 < cp_len) {
+		LOGP(DNAT, LOGL_ERROR, "SMS RPDU can not fit in: %d %d\n", cp_len, payload_len);
+		return NULL;
+	}
+
+	if (hdr48->data[1] != GSM411_MT_RP_DATA_MO)
+		return NULL;
+
+	if (cp_len < 5) {
+		LOGP(DNAT, LOGL_ERROR, "RD-DATA can not fit in the CP len: %d\n", cp_len);
+		return NULL;
+	}
+
+	ref = hdr48->data[2];
+	orig_addr_len = hdr48->data[3];
+	orig_addr_ptr = &hdr48->data[4];
+
+	/* the +1 is for checking if the following element has some space */
+	if (cp_len < 3 + orig_addr_len + 1) {
+		LOGP(DNAT, LOGL_ERROR, "RP-Originator addr does not fit: %d\n", orig_addr_len);
+		return NULL;
+	}
+
+	dest_addr_len = hdr48->data[3 + orig_addr_len + 1];
+	dest_addr_ptr = &hdr48->data[3 + orig_addr_len + 2];
+
+	if (cp_len < 3 + orig_addr_len + 1 + dest_addr_len + 1) {
+		LOGP(DNAT, LOGL_ERROR, "RP-Destination addr does not fit: %d\n", dest_addr_len);
+		return NULL;
+	}
+	gsm48_decode_bcd_number(smsc_addr, ARRAY_SIZE(smsc_addr), dest_addr_ptr - 1, 1);
+
+	data_len = hdr48->data[3 + orig_addr_len + 1 + dest_addr_len + 1];
+	data_ptr = &hdr48->data[3 + orig_addr_len + 1 + dest_addr_len + 2];
+
+	if (cp_len < 3 + orig_addr_len + 1 + dest_addr_len + 1 + data_len) {
+		LOGP(DNAT, LOGL_ERROR, "RP-Data does not fit: %d\n", data_len);
+		return NULL;
+	}
+
+	/* We will find a new number now */
+	llist_for_each_entry(entry, &nat->smsc_rewr, list) {
+		regmatch_t matches[2];
+
+		/* check the IMSI match */
+		if (regexec(&entry->msisdn_reg, imsi, 0, NULL, 0) != 0)
+			continue;
+
+		/* this regexp matches... */
+		if (regexec(&entry->num_reg, smsc_addr, 2, matches, 0) == 0 &&
+		    matches[1].rm_eo != -1)
+			new_number = talloc_asprintf(msg, "%s%s",
+					entry->replace,
+					&smsc_addr[matches[1].rm_so]);
+		if (new_number)
+			break;
+	}
+
+	if (!new_number)
+		return NULL;
+
+	/*
+	 * We need to re-create the patched structure. This is why we have
+	 * saved the above pointers.
+	 */
+	out = msgb_alloc_headroom(4096, 128, "changed-smsc");
+	if (!out) {
+		LOGP(DNAT, LOGL_ERROR, "Failed to allocate.\n");
+		return NULL;
+	}
+
+	out->l3h = out->data;
+	msgb_v_put(out, GSM411_MT_RP_DATA_MO);
+	msgb_v_put(out, ref);
+	msgb_lv_put(out, orig_addr_len, orig_addr_ptr);
+
+	/*
+	 * Copy the new number. We let libosmocore encode it, then set
+	 * the extension followed after the length. For our convenience
+	 * we let the TLV code re-add the length so we start copying
+	 * from &new_addr[1].
+	 */
+	new_addr_len = gsm48_encode_bcd_number(new_addr, ARRAY_SIZE(new_addr),
+					       1, new_number);
+	new_addr[1] = 0x91;
+	msgb_lv_put(out, new_addr_len - 1, new_addr + 1);
+
+	msgb_lv_put(out, data_len, data_ptr);
+
+	new_hdr48 = (struct gsm48_hdr *) msgb_push(out, sizeof(*hdr48) + 1);
+	memcpy(new_hdr48, hdr48, sizeof(*hdr48));
+	new_hdr48->data[0] = msgb_l3len(out);
+
+	talloc_free(new_number);
+	return out;
+}
+
 struct msgb *bsc_nat_rewrite_msg(struct bsc_nat *nat, struct msgb *msg, struct bsc_nat_parsed *parsed, const char *imsi)
 {
 	struct gsm48_hdr *hdr48;
@@ -944,6 +1070,8 @@ struct msgb *bsc_nat_rewrite_msg(struct bsc_nat *nat, struct msgb *msg, struct b
 
 	if (proto == GSM48_PDISC_CC && msg_type == GSM48_MT_CC_SETUP)
 		new_msg = rewrite_setup(nat, msg, parsed, imsi, hdr48, len);
+	else if (proto == GSM48_PDISC_SMS && msg_type == GSM411_MT_CP_DATA)
+		new_msg = rewrite_smsc(nat, msg, parsed, imsi, hdr48, len);
 
 	if (!new_msg)
 		return msg;
@@ -973,13 +1101,14 @@ static void num_rewr_free_data(struct bsc_nat_num_rewr_entry *entry)
 	talloc_free(entry->replace);
 }
 
-void bsc_nat_num_rewr_entry_adapt(struct bsc_nat *nat, const struct osmo_config_list *list)
+void bsc_nat_num_rewr_entry_adapt(void *ctx, struct llist_head *head,
+				  const struct osmo_config_list *list)
 {
 	struct bsc_nat_num_rewr_entry *entry, *tmp;
 	struct osmo_config_entry *cfg_entry;
 
 	/* free the old data */
-	llist_for_each_entry_safe(entry, tmp, &nat->num_rewr, list) {
+	llist_for_each_entry_safe(entry, tmp, head, list) {
 		num_rewr_free_data(entry);
 		llist_del(&entry->list);
 		talloc_free(entry);
@@ -997,7 +1126,7 @@ void bsc_nat_num_rewr_entry_adapt(struct bsc_nat *nat, const struct osmo_config_
 			continue;
 		}
 
-		entry = talloc_zero(nat, struct bsc_nat_num_rewr_entry);
+		entry = talloc_zero(ctx, struct bsc_nat_num_rewr_entry);
 		if (!entry) {
 			LOGP(DNAT, LOGL_ERROR,
 				"Allication of the num_rewr entry failed.\n");
@@ -1047,6 +1176,6 @@ void bsc_nat_num_rewr_entry_adapt(struct bsc_nat *nat, const struct osmo_config_
 		}
 
 		/* we have copied the number */
-		llist_add_tail(&entry->list, &nat->num_rewr);
+		llist_add_tail(&entry->list, head);
 	}
 }
diff --git a/openbsc/src/osmo-bsc_nat/bsc_nat_vty.c b/openbsc/src/osmo-bsc_nat/bsc_nat_vty.c
index 38b5a0905..f05ccaab1 100644
--- a/openbsc/src/osmo-bsc_nat/bsc_nat_vty.c
+++ b/openbsc/src/osmo-bsc_nat/bsc_nat_vty.c
@@ -466,11 +466,11 @@ DEFUN(cfg_nat_number_rewrite,
 	bsc_replace_string(_nat, &_nat->num_rewr_name, argv[0]);
 	if (_nat->num_rewr_name) {
 		rewr = osmo_config_list_parse(_nat, _nat->num_rewr_name);
-		bsc_nat_num_rewr_entry_adapt(_nat, rewr);
+		bsc_nat_num_rewr_entry_adapt(_nat, &_nat->num_rewr, rewr);
 		talloc_free(rewr);
 		return CMD_SUCCESS;
 	} else {
-		bsc_nat_num_rewr_entry_adapt(_nat, NULL);
+		bsc_nat_num_rewr_entry_adapt(_nat, &_nat->num_rewr, NULL);
 		return CMD_SUCCESS;
 	}
 }