nat: Look into the TPDU/SMS-SUBMIT and use the TP-DestAddress for matches

Match the used SMSC and the destination of the SMS and change
the SMSC address if both are matched.
diff --git a/openbsc/include/openbsc/bsc_nat.h b/openbsc/include/openbsc/bsc_nat.h
index 4506160..97fa4fe 100644
--- a/openbsc/include/openbsc/bsc_nat.h
+++ b/openbsc/include/openbsc/bsc_nat.h
@@ -271,6 +271,8 @@
 
 	char *smsc_rewr_name;
 	struct llist_head smsc_rewr;
+	char *tpdest_match_name;
+	struct llist_head tpdest_match;
 
 	/* USSD messages  we want to match */
 	char *ussd_lst_name;
diff --git a/openbsc/src/osmo-bsc_nat/bsc_nat_utils.c b/openbsc/src/osmo-bsc_nat/bsc_nat_utils.c
index ae9891e..4834340 100644
--- a/openbsc/src/osmo-bsc_nat/bsc_nat_utils.c
+++ b/openbsc/src/osmo-bsc_nat/bsc_nat_utils.c
@@ -97,6 +97,7 @@
 	INIT_LLIST_HEAD(&nat->dests);
 	INIT_LLIST_HEAD(&nat->num_rewr);
 	INIT_LLIST_HEAD(&nat->smsc_rewr);
+	INIT_LLIST_HEAD(&nat->tpdest_match);
 
 	nat->stats.sccp.conn = osmo_counter_alloc("nat.sccp.conn");
 	nat->stats.sccp.calls = osmo_counter_alloc("nat.sccp.calls");
@@ -935,6 +936,12 @@
 	char smsc_addr[30];
 	uint8_t new_addr[12];
 
+
+	uint8_t dest_len;
+	char _dest_nr[30];
+	char *dest_nr;
+	uint8_t dest_match = 0;
+
 	struct bsc_nat_num_rewr_entry *entry;
 	char *new_number = NULL;
 	uint8_t new_addr_len;
@@ -988,6 +995,40 @@
 		return NULL;
 	}
 
+	/* look into the phone number */
+	if ((data_ptr[0] & 0x01) != 1)
+		return NULL;
+
+	if (data_len < 3) {
+		LOGP(DNAT, LOGL_ERROR, "SMS-SUBMIT is too short.\n");
+		return NULL;
+	}
+
+	dest_len = data_ptr[2];
+	if (data_len < dest_len + 3 || dest_len < 2) {
+		LOGP(DNAT, LOGL_ERROR, "SMS-SUBMIT can not have TP-DestAddr.\n");
+		return NULL;
+	}
+
+	if ((data_ptr[3] & 0x80) == 0) {
+		LOGP(DNAT, LOGL_ERROR, "TP-DestAddr has extension. Not handled.\n");
+		return NULL;
+	}
+
+	if ((data_ptr[3] & 0x0F) == 0) {
+		LOGP(DNAT, LOGL_ERROR, "TP-DestAddr is not a ISDN number.\n");
+		return NULL;
+	}
+
+	gsm48_decode_bcd_number(_dest_nr + 2, ARRAY_SIZE(_dest_nr) - 2,
+				&data_ptr[2], 1);
+	if ((data_ptr[3] & 0x70) == 0x10) {
+		_dest_nr[0] = _dest_nr[1] = '0';
+		dest_nr = &_dest_nr[0];
+	} else {
+		dest_nr = &_dest_nr[2];
+	}
+
 	/* We will find a new number now */
 	llist_for_each_entry(entry, &nat->smsc_rewr, list) {
 		regmatch_t matches[2];
@@ -1010,6 +1051,25 @@
 		return NULL;
 
 	/*
+	 * now match the number against another list
+	 */
+	llist_for_each_entry(entry, &nat->tpdest_match, list) {
+		/* check the IMSI match */
+		if (regexec(&entry->msisdn_reg, imsi, 0, NULL, 0) != 0)
+			continue;
+
+		if (regexec(&entry->num_reg, dest_nr, 0, NULL, 0) == 0) {
+			dest_match =1;
+			break;
+		}
+	}
+
+	if (!dest_match) {
+		talloc_free(new_number);
+		return NULL;
+	}
+
+	/*
 	 * We need to re-create the patched structure. This is why we have
 	 * saved the above pointers.
 	 */
diff --git a/openbsc/tests/bsc-nat/bsc_nat_test.c b/openbsc/tests/bsc-nat/bsc_nat_test.c
index a31efca..d198424 100644
--- a/openbsc/tests/bsc-nat/bsc_nat_test.c
+++ b/openbsc/tests/bsc-nat/bsc_nat_test.c
@@ -989,15 +989,23 @@
 	struct bsc_nat *nat = bsc_nat_alloc();
 
 	/* a fake list */
-	struct osmo_config_list entries;
-	struct osmo_config_entry entry;
+	struct osmo_config_list smsc_entries, dest_entries;
+	struct osmo_config_entry smsc_entry, dest_entry;
 
-	INIT_LLIST_HEAD(&entries.entry);
-	entry.mcc = "^515039";
-	entry.option = "639180000105()";
-	entry.text   = "6666666666667";
-	llist_add_tail(&entry.list, &entries.entry);
-	bsc_nat_num_rewr_entry_adapt(nat, &nat->smsc_rewr, &entries);
+	INIT_LLIST_HEAD(&smsc_entries.entry);
+	INIT_LLIST_HEAD(&dest_entries.entry);
+	smsc_entry.mcc = "^515039";
+	smsc_entry.option = "639180000105()";
+	smsc_entry.text   = "6666666666667";
+	llist_add_tail(&smsc_entry.list, &smsc_entries.entry);
+	dest_entry.mcc = "515";
+	dest_entry.mnc = "03";
+	dest_entry.option = "^0049";
+	dest_entry.text   = "";
+	llist_add_tail(&dest_entry.list, &dest_entries.entry);
+
+	bsc_nat_num_rewr_entry_adapt(nat, &nat->smsc_rewr, &smsc_entries);
+	bsc_nat_num_rewr_entry_adapt(nat, &nat->tpdest_match, &dest_entries);
 
 	copy_to_msg(msg, smsc_rewrite, ARRAY_SIZE(smsc_rewrite));
 	parsed = bsc_nat_parse(msg);