New upstream version 0.9.14upstream/0.9.14

9 files changed, 1079 insertions, 327 deletions

diff --git a/src/osmo-bsc_nat/Makefile.am b/src/osmo-bsc_nat/Makefile.am
index 98a343108..03fe62b4b 100644
--- a/src/osmo-bsc_nat/Makefile.am
+++ b/src/osmo-bsc_nat/Makefile.am
@@ -1,6 +1,6 @@
 INCLUDES = $(all_includes) -I$(top_srcdir)/include -I$(top_builddir)
-AM_CFLAGS=-Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOVTY_CFLAGS) $(LIBOSMOSCCP_CFLAGS) $(COVERAGE_CFLAGS)
-AM_LDFLAGS = $(LIBOSMOCORE_LIBS) $(LIBOSMOVTY_LIBS) $(COVERAGE_LDFLAGS)
+AM_CFLAGS=-Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS) $(LIBOSMOVTY_CFLAGS) $(LIBOSMOSCCP_CFLAGS) $(COVERAGE_CFLAGS)
+AM_LDFLAGS = $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS) $(LIBOSMOVTY_LIBS) $(COVERAGE_LDFLAGS)
 
 bin_PROGRAMS = osmo-bsc_nat
 
diff --git a/src/osmo-bsc_nat/Makefile.in b/src/osmo-bsc_nat/Makefile.in
index ba69f7edb..6a51109c9 100644
--- a/src/osmo-bsc_nat/Makefile.in
+++ b/src/osmo-bsc_nat/Makefile.in
@@ -36,7 +36,7 @@ bin_PROGRAMS = osmo-bsc_nat$(EXEEXT)
 subdir = src/osmo-bsc_nat
 DIST_COMMON = $(srcdir)/Makefile.am $(srcdir)/Makefile.in
 ACLOCAL_M4 = $(top_srcdir)/aclocal.m4
-am__aclocal_m4_deps = $(top_srcdir)/configure.in
+am__aclocal_m4_deps = $(top_srcdir)/configure.ac
 am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \
 	$(ACLOCAL_M4)
 mkinstalldirs = $(install_sh) -d
@@ -114,6 +114,8 @@ LDFLAGS = @LDFLAGS@
 LIBOBJS = @LIBOBJS@
 LIBOSMOCORE_CFLAGS = @LIBOSMOCORE_CFLAGS@
 LIBOSMOCORE_LIBS = @LIBOSMOCORE_LIBS@
+LIBOSMOGSM_CFLAGS = @LIBOSMOGSM_CFLAGS@
+LIBOSMOGSM_LIBS = @LIBOSMOGSM_LIBS@
 LIBOSMOSCCP_CFLAGS = @LIBOSMOSCCP_CFLAGS@
 LIBOSMOSCCP_LIBS = @LIBOSMOSCCP_LIBS@
 LIBOSMOVTY_CFLAGS = @LIBOSMOVTY_CFLAGS@
@@ -183,8 +185,8 @@ top_build_prefix = @top_build_prefix@
 top_builddir = @top_builddir@
 top_srcdir = @top_srcdir@
 INCLUDES = $(all_includes) -I$(top_srcdir)/include -I$(top_builddir)
-AM_CFLAGS = -Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOVTY_CFLAGS) $(LIBOSMOSCCP_CFLAGS) $(COVERAGE_CFLAGS)
-AM_LDFLAGS = $(LIBOSMOCORE_LIBS) $(LIBOSMOVTY_LIBS) $(COVERAGE_LDFLAGS)
+AM_CFLAGS = -Wall $(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS) $(LIBOSMOVTY_CFLAGS) $(LIBOSMOSCCP_CFLAGS) $(COVERAGE_CFLAGS)
+AM_LDFLAGS = $(LIBOSMOCORE_LIBS) $(LIBOSMOGSM_LIBS) $(LIBOSMOVTY_LIBS) $(COVERAGE_LDFLAGS)
 osmo_bsc_nat_SOURCES = bsc_filter.c bsc_mgcp_utils.c bsc_nat.c bsc_nat_utils.c \
 		  bsc_nat_vty.c bsc_sccp.c bsc_ussd.c
 
diff --git a/src/osmo-bsc_nat/bsc_filter.c b/src/osmo-bsc_nat/bsc_filter.c
index 73e987893..74a5d193f 100644
--- a/src/osmo-bsc_nat/bsc_filter.c
+++ b/src/osmo-bsc_nat/bsc_filter.c
@@ -25,8 +25,8 @@
 #include <openbsc/ipaccess.h>
 #include <openbsc/debug.h>
 
-#include <osmocore/talloc.h>
-#include <osmocore/protocol/gsm_08_08.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/gsm/protocol/gsm_08_08.h>
 
 #include <osmocom/sccp/sccp.h>
 
diff --git a/src/osmo-bsc_nat/bsc_mgcp_utils.c b/src/osmo-bsc_nat/bsc_mgcp_utils.c
index 9eac00bf4..9ac54dabe 100644
--- a/src/osmo-bsc_nat/bsc_mgcp_utils.c
+++ b/src/osmo-bsc_nat/bsc_mgcp_utils.c
@@ -28,9 +28,9 @@
 
 #include <osmocom/sccp/sccp.h>
 
-#include <osmocore/talloc.h>
-#include <osmocore/gsm0808.h>
-#include <osmocore/protocol/gsm_08_08.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/gsm/gsm0808.h>
+#include <osmocom/gsm/protocol/gsm_08_08.h>
 
 #include <netinet/in.h>
 #include <arpa/inet.h>
@@ -478,7 +478,7 @@ void bsc_mgcp_forward(struct bsc_connection *bsc, struct msgb *msg)
 		return;
 	}
 
-	if (write_queue_enqueue(&bsc->nat->mgcp_cfg->gw_fd, output) != 0) {
+	if (osmo_wqueue_enqueue(&bsc->nat->mgcp_cfg->gw_fd, output) != 0) {
 		LOGP(DMGCP, LOGL_ERROR, "Failed to queue MGCP msg.\n");
 		msgb_free(output);
 	}
@@ -598,7 +598,7 @@ struct msgb *bsc_mgcp_rewrite(char *input, int length, int endpoint, const char
 	return output;
 }
 
-static int mgcp_do_read(struct bsc_fd *fd)
+static int mgcp_do_read(struct osmo_fd *fd)
 {
 	struct bsc_nat *nat;
 	struct msgb *msg, *resp;
@@ -628,7 +628,7 @@ static int mgcp_do_read(struct bsc_fd *fd)
 
 	/* we do have a direct answer... e.g. AUEP */
 	if (resp) {
-		if (write_queue_enqueue(&nat->mgcp_cfg->gw_fd, resp) != 0) {
+		if (osmo_wqueue_enqueue(&nat->mgcp_cfg->gw_fd, resp) != 0) {
 			LOGP(DMGCP, LOGL_ERROR, "Failed to enqueue msg.\n");
 			msgb_free(resp);
 		}
@@ -637,7 +637,7 @@ static int mgcp_do_read(struct bsc_fd *fd)
 	return 0;
 }
 
-static int mgcp_do_write(struct bsc_fd *bfd, struct msgb *msg)
+static int mgcp_do_write(struct osmo_fd *bfd, struct msgb *msg)
 {
 	int rc;
 
@@ -681,7 +681,8 @@ int bsc_mgcp_nat_init(struct bsc_nat *nat)
 	inet_aton(cfg->source_addr, &addr.sin_addr);
 
 	if (bind(cfg->gw_fd.bfd.fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
-		LOGP(DMGCP, LOGL_ERROR, "Failed to bind. errno: %d\n", errno);
+		LOGP(DMGCP, LOGL_ERROR, "Failed to bind on %s:%d errno: %d\n",
+		     cfg->source_addr, cfg->source_port, errno);
 		close(cfg->gw_fd.bfd.fd);
 		cfg->gw_fd.bfd.fd = -1;
 		return -1;
@@ -697,13 +698,13 @@ int bsc_mgcp_nat_init(struct bsc_nat *nat)
 		return -1;
 	}
 
-	write_queue_init(&cfg->gw_fd, 10);
+	osmo_wqueue_init(&cfg->gw_fd, 10);
 	cfg->gw_fd.bfd.when = BSC_FD_READ;
 	cfg->gw_fd.bfd.data = nat;
 	cfg->gw_fd.read_cb = mgcp_do_read;
 	cfg->gw_fd.write_cb = mgcp_do_write;
 
-	if (bsc_register_fd(&cfg->gw_fd.bfd) != 0) {
+	if (osmo_fd_register(&cfg->gw_fd.bfd) != 0) {
 		LOGP(DMGCP, LOGL_ERROR, "Failed to register MGCP fd.\n");
 		close(cfg->gw_fd.bfd.fd);
 		cfg->gw_fd.bfd.fd = -1;
diff --git a/src/osmo-bsc_nat/bsc_nat.c b/src/osmo-bsc_nat/bsc_nat.c
index 643b3c4ba..e0eb635f0 100644
--- a/src/osmo-bsc_nat/bsc_nat.c
+++ b/src/osmo-bsc_nat/bsc_nat.c
@@ -1,8 +1,8 @@
 /* BSC Multiplexer/NAT */
 
 /*
- * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
- * (C) 2010 by On-Waves
+ * (C) 2010-2011 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2010-2011 by On-Waves
  * (C) 2009 by Harald Welte <laforge@gnumonks.org>
  * All Rights Reserved
  *
@@ -44,14 +44,16 @@
 #include <openbsc/socket.h>
 #include <openbsc/vty.h>
 
-#include <osmocore/gsm0808.h>
-#include <osmocore/talloc.h>
-#include <osmocore/process.h>
+#include <osmocom/core/application.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/process.h>
 
-#include <osmocore/protocol/gsm_08_08.h>
+#include <osmocom/gsm/gsm0808.h>
+#include <osmocom/gsm/protocol/gsm_08_08.h>
 
 #include <osmocom/vty/telnet_interface.h>
 #include <osmocom/vty/vty.h>
+#include <osmocom/vty/logging.h>
 
 #include <osmocom/sccp/sccp.h>
 
@@ -60,12 +62,11 @@
 #define SCCP_CLOSE_TIME 20
 #define SCCP_CLOSE_TIME_TIMEOUT 19
 
-struct log_target *stderr_target;
 static const char *config_file = "bsc-nat.cfg";
 static struct in_addr local_addr;
-static struct bsc_fd bsc_listen;
+static struct osmo_fd bsc_listen;
 static const char *msc_ip = NULL;
-static struct timer_list sccp_close;
+static struct osmo_timer_list sccp_close;
 static int daemonize = 0;
 
 const char *openbsc_copyright =
@@ -99,7 +100,7 @@ static void queue_for_msc(struct bsc_msc_connection *con, struct msgb *msg)
 	}
 
 
-	if (write_queue_enqueue(&con->write_queue, msg) != 0) {
+	if (osmo_wqueue_enqueue(&con->write_queue, msg) != 0) {
 		LOGP(DINP, LOGL_ERROR, "Failed to enqueue the write.\n");
 		msgb_free(msg);
 	}
@@ -152,10 +153,10 @@ static void bsc_ping_timeout(void *_bsc)
 	send_ping(bsc);
 
 	/* send another ping in 20 seconds */
-	bsc_schedule_timer(&bsc->ping_timeout, bsc->nat->ping_timeout, 0);
+	osmo_timer_schedule(&bsc->ping_timeout, bsc->nat->ping_timeout, 0);
 
 	/* also start a pong timer */
-	bsc_schedule_timer(&bsc->pong_timeout, bsc->nat->pong_timeout, 0);
+	osmo_timer_schedule(&bsc->pong_timeout, bsc->nat->pong_timeout, 0);
 }
 
 static void start_ping_pong(struct bsc_connection *bsc)
@@ -194,15 +195,15 @@ static void send_id_req(struct bsc_connection *bsc)
 	bsc_send_data(bsc, id_req, sizeof(id_req), IPAC_PROTO_IPACCESS);
 }
 
-static void nat_send_rlsd_msc(struct sccp_connections *conn)
+static struct msgb *nat_create_rlsd(struct sccp_connections *conn)
 {
 	struct sccp_connection_released *rel;
 	struct msgb *msg;
 
 	msg = msgb_alloc_headroom(4096, 128, "rlsd");
 	if (!msg) {
-		LOGP(DNAT, LOGL_ERROR, "Failed to allocate clear command.\n");
-		return;
+		LOGP(DNAT, LOGL_ERROR, "Failed to allocate released.\n");
+		return NULL;
 	}
 
 	msg->l2h = msgb_put(msg, sizeof(*rel));
@@ -212,15 +213,39 @@ static void nat_send_rlsd_msc(struct sccp_connections *conn)
 	rel->destination_local_reference = conn->remote_ref;
 	rel->source_local_reference = conn->patched_ref;
 
-	ipaccess_prepend_header(msg, IPAC_PROTO_SCCP);
+	return msg;
+}
+
+static void nat_send_rlsd_ussd(struct bsc_nat *nat, struct sccp_connections *conn)
+{
+	struct msgb *msg;
+
+	if (!nat->ussd_con)
+		return;
+
+	msg = nat_create_rlsd(conn);
+	if (!msg)
+		return;
+
+	bsc_do_write(&nat->ussd_con->queue, msg, IPAC_PROTO_SCCP);
+}
+
+static void nat_send_rlsd_msc(struct sccp_connections *conn)
+{
+	struct msgb *msg;
 
+	msg = nat_create_rlsd(conn);
+	if (!msg)
+		return;
+
+	ipaccess_prepend_header(msg, IPAC_PROTO_SCCP);
 	queue_for_msc(conn->msc_con, msg);
 }
 
 static void nat_send_rlsd_bsc(struct sccp_connections *conn)
 {
-	struct sccp_connection_released *rel;
 	struct msgb *msg;
+	struct sccp_connection_released *rel;
 
 	msg = msgb_alloc_headroom(4096, 128, "rlsd");
 	if (!msg) {
@@ -398,7 +423,7 @@ static void bsc_send_con_release(struct bsc_connection *bsc, struct sccp_connect
 		ipaccess_prepend_header(rlsd, IPAC_PROTO_SCCP);
 		queue_for_msc(con->msc_con, rlsd);
 	}
-	con->con_local = 1;
+	con->con_local = NAT_CON_END_LOCAL;
 	con->msc_con = NULL;
 
 	/* 2. release the BSC side */
@@ -464,7 +489,7 @@ static void bsc_send_con_refuse(struct bsc_connection *bsc,
 
 		/* declare it local and assign a unique remote_ref */
 		con->con_type = NAT_CON_TYPE_LOCAL_REJECT;
-		con->con_local = 1;
+		con->con_local = NAT_CON_END_LOCAL;
 		con->has_remote_ref = 1;
 		con->remote_ref = con->patched_ref;
 
@@ -524,6 +549,82 @@ send_refuse:
 	bsc_write(bsc, refuse, IPAC_PROTO_SCCP);
 }
 
+static void bsc_nat_send_paging(struct bsc_connection *bsc, struct msgb *msg)
+{
+	if (bsc->cfg->forbid_paging) {
+		LOGP(DNAT, LOGL_DEBUG, "Paging forbidden for BTS: %d\n", bsc->cfg->nr);
+		return;
+	}
+
+	bsc_send_data(bsc, msg->l2h, msgb_l2len(msg), IPAC_PROTO_SCCP);
+}
+
+static void bsc_nat_handle_paging(struct bsc_nat *nat, struct msgb *msg)
+{
+	struct bsc_connection *bsc;
+	const uint8_t *paging_start;
+	int paging_length, i, ret;
+
+	ret = bsc_nat_find_paging(msg, &paging_start, &paging_length);
+	if (ret != 0) {
+		LOGP(DNAT, LOGL_ERROR, "Could not parse paging message: %d\n", ret);
+		return;
+	}
+
+	/* This is quite expensive now */
+	for (i = 0; i < paging_length; i += 2) {
+		unsigned int _lac = ntohs(*(unsigned int *) &paging_start[i]);
+		unsigned int paged = 0;
+		llist_for_each_entry(bsc, &nat->bsc_connections, list_entry) {
+			if (!bsc->cfg)
+				continue;
+			if (!bsc->authenticated)
+				continue;
+			if (!bsc_config_handles_lac(bsc->cfg, _lac))
+				continue;
+			bsc_nat_send_paging(bsc, msg);
+			paged += 1;
+		}
+
+		/* highlight a possible config issue */
+		if (paged == 0)
+			LOGP(DNAT, LOGL_ERROR, "No BSC for LAC %d/0x%d\n", _lac, _lac);
+
+	}
+}
+
+
+/*
+ * Update the auth status. This can be either a CIPHER MODE COMAMND or
+ * a CM Serivce Accept. Maybe also LU Accept or such in the future.
+ */
+static void update_con_authorize(struct sccp_connections *con,
+				 struct bsc_nat_parsed *parsed,
+				 struct msgb *msg)
+{
+	if (!con)
+		return;
+	if (con->authorized)
+		return;
+
+	if (parsed->bssap == BSSAP_MSG_BSS_MANAGEMENT &&
+	    parsed->gsm_type == BSS_MAP_MSG_CIPHER_MODE_CMD) {
+		con->authorized = 1;
+	} else if (parsed->bssap == BSSAP_MSG_DTAP) {
+		uint8_t msg_type, proto;
+		uint32_t len;
+		struct gsm48_hdr *hdr48;
+		hdr48 = bsc_unpack_dtap(parsed, msg, &len);
+		if (!hdr48)
+			return;
+
+		proto = hdr48->proto_discr & 0x0f;
+		msg_type = hdr48->msg_type & 0xbf;
+		if (proto == GSM48_PDISC_MM &&
+		    msg_type == GSM48_MT_MM_CM_SERV_ACC)
+			con->authorized = 1;
+	}
+}
 
 static int forward_sccp_to_bts(struct bsc_msc_connection *msc_con, struct msgb *msg)
 {
@@ -552,12 +653,17 @@ static int forward_sccp_to_bts(struct bsc_msc_connection *msc_con, struct msgb *
 			goto send_to_all;
 			break;
 		case SCCP_MSG_TYPE_RLSD:
+			if (con && con->con_local == NAT_CON_END_USSD) {
+				LOGP(DNAT, LOGL_NOTICE, "RLSD for a USSD connection. Ignoring.\n");
+				con = NULL;
+			}
+			/* fall through */
 		case SCCP_MSG_TYPE_CREF:
 		case SCCP_MSG_TYPE_DT1:
 		case SCCP_MSG_TYPE_IT:
 			con = patch_sccp_src_ref_to_bsc(msg, parsed, nat);
 			if (parsed->gsm_type == BSS_MAP_MSG_ASSIGMENT_RQST) {
-				counter_inc(nat->stats.sccp.calls);
+				osmo_counter_inc(nat->stats.sccp.calls);
 
 				if (con) {
 					struct rate_ctr_group *ctrg;
@@ -567,6 +673,10 @@ static int forward_sccp_to_bts(struct bsc_msc_connection *msc_con, struct msgb *
 						LOGP(DNAT, LOGL_ERROR, "Failed to assign...\n");
 				} else
 					LOGP(DNAT, LOGL_ERROR, "Assignment command but no BSC.\n");
+			} else if (con && con->con_local == NAT_CON_END_USSD &&
+				   parsed->gsm_type == BSS_MAP_MSG_CLEAR_CMD) {
+				LOGP(DNAT, LOGL_NOTICE, "Clear Command for USSD Connection. Ignoring.\n");
+				con = NULL;
 			}
 			break;
 		case SCCP_MSG_TYPE_CC:
@@ -600,6 +710,8 @@ static int forward_sccp_to_bts(struct bsc_msc_connection *msc_con, struct msgb *
 		return -1;
 	}
 
+	update_con_authorize(con, parsed, msg);
+
 	bsc_send_data(con->bsc, msg->l2h, msgb_l2len(msg), proto);
 	return 0;
 
@@ -610,16 +722,7 @@ send_to_all:
 	 * message and then send it to the authenticated messages...
 	 */
 	if (parsed->ipa_proto == IPAC_PROTO_SCCP && parsed->gsm_type == BSS_MAP_MSG_PAGING) {
-		int lac;
-		bsc = bsc_nat_find_bsc(nat, msg, &lac);
-		if (bsc && bsc->cfg->forbid_paging)
-			LOGP(DNAT, LOGL_DEBUG, "Paging forbidden for BTS: %d\n", bsc->cfg->nr);
-		else if (bsc)
-			bsc_send_data(bsc, msg->l2h, msgb_l2len(msg), parsed->ipa_proto);
-		else if (lac != -1)
-			LOGP(DNAT, LOGL_ERROR, "Could not determine BSC for paging on lac: %d/0x%x\n",
-			     lac, lac);
-
+		bsc_nat_handle_paging(nat, msg);
 		goto exit;
 	}
 	/* currently send this to every BSC connected */
@@ -649,7 +752,7 @@ static void msc_connection_was_lost(struct bsc_msc_connection *con)
 
 static void msc_connection_connected(struct bsc_msc_connection *con)
 {
-	counter_inc(nat->stats.msc.reconn);
+	osmo_counter_inc(nat->stats.msc.reconn);
 }
 
 static void msc_send_reset(struct bsc_msc_connection *msc_con)
@@ -677,7 +780,7 @@ static void msc_send_reset(struct bsc_msc_connection *msc_con)
 	LOGP(DMSC, LOGL_NOTICE, "Scheduled GSM0808 reset msg for the MSC.\n");
 }
 
-static int ipaccess_msc_read_cb(struct bsc_fd *bfd)
+static int ipaccess_msc_read_cb(struct osmo_fd *bfd)
 {
 	int error;
 	struct bsc_msc_connection *msc_con;
@@ -696,7 +799,7 @@ static int ipaccess_msc_read_cb(struct bsc_fd *bfd)
 		return -1;
 	}
 
-	LOGP(DNAT, LOGL_DEBUG, "MSG from MSC: %s proto: %d\n", hexdump(msg->data, msg->len), msg->l2h[0]);
+	LOGP(DNAT, LOGL_DEBUG, "MSG from MSC: %s proto: %d\n", osmo_hexdump(msg->data, msg->len), msg->l2h[0]);
 
 	/* handle base message handling */
 	hh = (struct ipaccess_head *) msg->data;
@@ -715,7 +818,7 @@ static int ipaccess_msc_read_cb(struct bsc_fd *bfd)
 	return 0;
 }
 
-static int ipaccess_msc_write_cb(struct bsc_fd *bfd, struct msgb *msg)
+static int ipaccess_msc_write_cb(struct osmo_fd *bfd, struct msgb *msg)
 {
 	int rc;
 	rc = write(bfd->fd, msg->data, msg->len);
@@ -745,9 +848,9 @@ void bsc_close_connection(struct bsc_connection *connection)
 	struct rate_ctr *ctr = NULL;
 
 	/* stop the timeout timer */
-	bsc_del_timer(&connection->id_timeout);
-	bsc_del_timer(&connection->ping_timeout);
-	bsc_del_timer(&connection->pong_timeout);
+	osmo_timer_del(&connection->id_timeout);
+	osmo_timer_del(&connection->ping_timeout);
+	osmo_timer_del(&connection->pong_timeout);
 
 	if (connection->cfg)
 		ctr = &connection->cfg->stats.ctrg->ctr[BCFG_CTR_DROPPED_SCCP];
@@ -759,22 +862,44 @@ void bsc_close_connection(struct bsc_connection *connection)
 
 		if (ctr)
 			rate_ctr_inc(ctr);
-		if (sccp_patch->has_remote_ref && !sccp_patch->con_local)
-			nat_send_rlsd_msc(sccp_patch);
+		if (sccp_patch->has_remote_ref) {
+			if (sccp_patch->con_local == NAT_CON_END_MSC)
+				nat_send_rlsd_msc(sccp_patch);
+			else if (sccp_patch->con_local == NAT_CON_END_USSD)
+				nat_send_rlsd_ussd(nat, sccp_patch);
+		}
+
 		sccp_connection_destroy(sccp_patch);
 	}
 
 	/* close endpoints allocated by this BSC */
 	bsc_mgcp_clear_endpoints_for(connection);
 
-	bsc_unregister_fd(&connection->write_queue.bfd);
+	osmo_fd_unregister(&connection->write_queue.bfd);
 	close(connection->write_queue.bfd.fd);
-	write_queue_clear(&connection->write_queue);
+	osmo_wqueue_clear(&connection->write_queue);
 	llist_del(&connection->list_entry);
 
 	talloc_free(connection);
 }
 
+static void bsc_maybe_close(struct bsc_connection *bsc)
+{
+	struct sccp_connections *sccp;
+	if (!bsc->nat->blocked)
+		return;
+
+	/* are there any connections left */
+	llist_for_each_entry(sccp, &bsc->nat->sccp_connections, list_entry)
+		if (sccp->bsc == bsc)
+			return;
+
+	/* nothing left, close the BSC */
+	LOGP(DNAT, LOGL_NOTICE, "Cleaning up BSC %d in blocking mode.\n",
+	     bsc->cfg ? bsc->cfg->nr : -1);
+	bsc_close_connection(bsc);
+}
+
 static void ipaccess_close_bsc(void *data)
 {
 	struct sockaddr_in sock;
@@ -805,7 +930,7 @@ static void ipaccess_auth_bsc(struct tlv_parsed *tvp, struct bsc_connection *bsc
 			rate_ctr_inc(&conf->stats.ctrg->ctr[BCFG_CTR_NET_RECONN]);
 			bsc->authenticated = 1;
 			bsc->cfg = conf;
-			bsc_del_timer(&bsc->id_timeout);
+			osmo_timer_del(&bsc->id_timeout);
 			LOGP(DNAT, LOGL_NOTICE, "Authenticated bsc nr: %d on fd %d\n",
 			     conf->nr, bsc->write_queue.bfd.fd);
 			start_ping_pong(bsc);
@@ -908,16 +1033,18 @@ static int forward_sccp_to_msc(struct bsc_connection *bsc, struct msgb *msg)
 
 					/* hand data to a side channel */
 					if (bsc_check_ussd(con, parsed, msg) == 1) 
-						con->con_local = 2;
+						con->con_local = NAT_CON_END_USSD;
 
 					/*
 					 * Optionally rewrite setup message. This can
 					 * replace the msg and the parsed structure becomes
 					 * invalid.
 					 */
-					msg = bsc_nat_rewrite_setup(bsc->nat, msg, parsed, con->imsi);
+					msg = bsc_nat_rewrite_msg(bsc->nat, msg, parsed, con->imsi);
 					talloc_free(parsed);
 					parsed = NULL;
+				} else if (con->con_local == NAT_CON_END_USSD) {
+					bsc_check_ussd(con, parsed, msg);
 				}
 
 				con_bsc = con->bsc;
@@ -934,6 +1061,7 @@ static int forward_sccp_to_msc(struct bsc_connection *bsc, struct msgb *msg)
 				con_filter = con->con_local;
 			}
 			remove_sccp_src_ref(bsc, msg, parsed);
+			bsc_maybe_close(bsc);
 			break;
 		case SCCP_MSG_TYPE_UDT:
 			/* simply forward everything */
@@ -986,9 +1114,15 @@ exit:
 		/* do we know who is handling this? */
 		if (msg->l2h[0] == IPAC_MSGT_ID_RESP) {
 			struct tlv_parsed tvp;
-			ipaccess_idtag_parse(&tvp,
+			int ret;
+			ret = ipaccess_idtag_parse(&tvp,
 					     (unsigned char *) msg->l2h + 2,
 					     msgb_l2len(msg) - 2);
+			if (ret < 0) {
+				LOGP(DNAT, LOGL_ERROR, "ignoring IPA response "
+					"message with malformed TLVs\n");
+				return ret;
+			}
 			if (TLVP_PRESENT(&tvp, IPAC_IDTAG_UNITNAME))
 				ipaccess_auth_bsc(&tvp, bsc);
 		}
@@ -1013,7 +1147,7 @@ exit3:
 	return -1;
 }
 
-static int ipaccess_bsc_read_cb(struct bsc_fd *bfd)
+static int ipaccess_bsc_read_cb(struct osmo_fd *bfd)
 {
 	int error;
 	struct bsc_connection *bsc = bfd->data;
@@ -1035,7 +1169,7 @@ static int ipaccess_bsc_read_cb(struct bsc_fd *bfd)
 	}
 
 
-	LOGP(DNAT, LOGL_DEBUG, "MSG from BSC: %s proto: %d\n", hexdump(msg->data, msg->len), msg->l2h[0]);
+	LOGP(DNAT, LOGL_DEBUG, "MSG from BSC: %s proto: %d\n", osmo_hexdump(msg->data, msg->len), msg->l2h[0]);
 
 	/* Handle messages from the BSC */
 	hh = (struct ipaccess_head *) msg->data;
@@ -1043,7 +1177,7 @@ static int ipaccess_bsc_read_cb(struct bsc_fd *bfd)
 	/* stop the pong timeout */
 	if (hh->proto == IPAC_PROTO_IPACCESS) {
 		if (msg->l2h[0] == IPAC_MSGT_PONG) {
-			bsc_del_timer(&bsc->pong_timeout);
+			osmo_timer_del(&bsc->pong_timeout);
 			msgb_free(msg);
 			return 0;
 		} else if (msg->l2h[0] == IPAC_MSGT_PING) {
@@ -1060,7 +1194,7 @@ static int ipaccess_bsc_read_cb(struct bsc_fd *bfd)
 	return 0;
 }
 
-static int ipaccess_listen_bsc_cb(struct bsc_fd *bfd, unsigned int what)
+static int ipaccess_listen_bsc_cb(struct osmo_fd *bfd, unsigned int what)
 {
 	struct bsc_connection *bsc;
 	int fd, rc, on;
@@ -1077,7 +1211,7 @@ static int ipaccess_listen_bsc_cb(struct bsc_fd *bfd, unsigned int what)
 	}
 
 	/* count the reconnect */
-	counter_inc(nat->stats.bsc.reconn);
+	osmo_counter_inc(nat->stats.bsc.reconn);
 
 	/*
 	 * if we are not connected to a msc... just close the socket
@@ -1088,6 +1222,12 @@ static int ipaccess_listen_bsc_cb(struct bsc_fd *bfd, unsigned int what)
 		return 0;
 	}
 
+	if (nat->blocked) {
+		LOGP(DNAT, LOGL_NOTICE, "Disconnecting BSC due NAT being blocked.\n");
+		close(fd);
+		return 0;
+	}
+
 	on = 1;
 	rc = setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &on, sizeof(on));
 	if (rc != 0)
@@ -1116,7 +1256,7 @@ static int ipaccess_listen_bsc_cb(struct bsc_fd *bfd, unsigned int what)
 	bsc->write_queue.read_cb = ipaccess_bsc_read_cb;
 	bsc->write_queue.write_cb = bsc_write_cb;
 	bsc->write_queue.bfd.when = BSC_FD_READ;
-	if (bsc_register_fd(&bsc->write_queue.bfd) < 0) {
+	if (osmo_fd_register(&bsc->write_queue.bfd) < 0) {
 		LOGP(DNAT, LOGL_ERROR, "Failed to register BSC fd.\n");
 		close(fd);
 		talloc_free(bsc);
@@ -1135,7 +1275,7 @@ static int ipaccess_listen_bsc_cb(struct bsc_fd *bfd, unsigned int what)
 	 */
 	bsc->id_timeout.data = bsc;
 	bsc->id_timeout.cb = ipaccess_close_bsc;
-	bsc_schedule_timer(&bsc->id_timeout, nat->auth_timeout, 0);
+	osmo_timer_schedule(&bsc->id_timeout, nat->auth_timeout, 0);
 	return 0;
 }
 
@@ -1182,16 +1322,16 @@ static void handle_options(int argc, char **argv)
 			print_help();
 			exit(0);
 		case 's':
-			log_set_use_color(stderr_target, 0);
+			log_set_use_color(osmo_stderr_target, 0);
 			break;
 		case 'd':
-			log_parse_category_mask(stderr_target, optarg);
+			log_parse_category_mask(osmo_stderr_target, optarg);
 			break;
 		case 'c':
 			config_file = strdup(optarg);
 			break;
 		case 'T':
-			log_set_print_timestamp(stderr_target, 1);
+			log_set_print_timestamp(osmo_stderr_target, 1);
 			break;
 		case 'm':
 			msc_ip = optarg;
@@ -1222,6 +1362,8 @@ static void signal_handler(int signal)
 
 static void sccp_close_unconfirmed(void *_data)
 {
+	int destroyed = 0;
+	struct bsc_connection *bsc, *bsc_tmp;
 	struct sccp_connections *conn, *tmp1;
 	struct timespec now;
 	clock_gettime(CLOCK_MONOTONIC, &now);
@@ -1238,9 +1380,18 @@ static void sccp_close_unconfirmed(void *_data)
 		     sccp_src_ref_to_int(&conn->real_ref),
 		     sccp_src_ref_to_int(&conn->patched_ref));
 		sccp_connection_destroy(conn);
+		destroyed = 1;
 	}
 
-	bsc_schedule_timer(&sccp_close, SCCP_CLOSE_TIME, 0);
+	if (!destroyed)
+		goto out;
+
+	/* now close out any BSC */
+	llist_for_each_entry_safe(bsc, bsc_tmp, &nat->bsc_connections, list_entry)
+		bsc_maybe_close(bsc);
+
+out:
+	osmo_timer_schedule(&sccp_close, SCCP_CLOSE_TIME, 0);
 }
 
 extern void *tall_msgb_ctx;
@@ -1267,10 +1418,7 @@ int main(int argc, char **argv)
 
 	talloc_init_ctx();
 
-	log_init(&log_info);
-	stderr_target = log_target_create_stderr();
-	log_add_target(stderr_target);
-	log_set_all_filter(stderr_target, 1);
+	osmo_init_logging(&log_info);
 
 	nat = bsc_nat_alloc();
 	if (!nat) {
@@ -1286,7 +1434,7 @@ int main(int argc, char **argv)
 
 	vty_info.copyright = openbsc_copyright;
 	vty_init(&vty_info);
-	logging_vty_add_cmds();
+	logging_vty_add_cmds(&log_info);
 	bsc_nat_vty_init(nat);
 
 
@@ -1317,7 +1465,7 @@ int main(int argc, char **argv)
 		return -4;
 
 	/* connect to the MSC */
-	nat->msc_con = bsc_msc_create(nat->msc_ip, nat->msc_port, 0);
+	nat->msc_con = bsc_msc_create(nat, &nat->dests);
 	if (!nat->msc_con) {
 		fprintf(stderr, "Creating a bsc_msc_connection failed.\n");
 		exit(1);
@@ -1332,7 +1480,7 @@ int main(int argc, char **argv)
 
 	/* wait for the BSC */
 	rc = make_sock(&bsc_listen, IPPROTO_TCP, ntohl(local_addr.s_addr),
-		       5000, ipaccess_listen_bsc_cb);
+		       5000, 0, ipaccess_listen_bsc_cb, nat);
 	if (rc != 0) {
 		fprintf(stderr, "Failed to listen for BSC.\n");
 		exit(1);
@@ -1346,7 +1494,7 @@ int main(int argc, char **argv)
 
 	signal(SIGABRT, &signal_handler);
 	signal(SIGUSR1, &signal_handler);
-	signal(SIGPIPE, SIG_IGN);
+	osmo_init_ignore_signals();
 
 	if (daemonize) {
 		rc = osmo_daemonize();
@@ -1360,10 +1508,10 @@ int main(int argc, char **argv)
 	sccp_set_log_area(DSCCP);
 	sccp_close.cb = sccp_close_unconfirmed;
 	sccp_close.data = NULL;
-	bsc_schedule_timer(&sccp_close, SCCP_CLOSE_TIME, 0);
+	osmo_timer_schedule(&sccp_close, SCCP_CLOSE_TIME, 0);
 
 	while (1) {
-		bsc_select_main(0);
+		osmo_select_main(0);
 	}
 
 	return 0;
@@ -1374,7 +1522,7 @@ int bsc_close_ussd_connections(struct bsc_nat *nat)
 {
 	struct sccp_connections *con;
 	llist_for_each_entry(con, &nat->sccp_connections, list_entry) {
-		if (con->con_local != 2)
+		if (con->con_local != NAT_CON_END_USSD)
 			continue;
 		if (!con->bsc)
 			continue;
diff --git a/src/osmo-bsc_nat/bsc_nat_utils.c b/src/osmo-bsc_nat/bsc_nat_utils.c
index cd294ccfb..4834340c8 100644
--- a/src/osmo-bsc_nat/bsc_nat_utils.c
+++ b/src/osmo-bsc_nat/bsc_nat_utils.c
@@ -2,8 +2,8 @@
 /* BSC Multiplexer/NAT Utilities */
 
 /*
- * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
- * (C) 2010 by On-Waves
+ * (C) 2010-2011 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2010-2011 by On-Waves
  * All Rights Reserved
  *
  * This program is free software; you can redistribute it and/or modify
@@ -29,11 +29,12 @@
 #include <openbsc/ipaccess.h>
 #include <openbsc/vty.h>
 
-#include <osmocore/linuxlist.h>
-#include <osmocore/talloc.h>
-#include <osmocore/gsm0808.h>
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/gsm/gsm0808.h>
 
-#include <osmocore/protocol/gsm_08_08.h>
+#include <osmocom/gsm/protocol/gsm_08_08.h>
+#include <osmocom/gsm/protocol/gsm_04_11.h>
 
 #include <osmocom/sccp/sccp.h>
 
@@ -82,28 +83,42 @@ struct bsc_nat *bsc_nat_alloc(void)
 	if (!nat)
 		return NULL;
 
+	nat->main_dest = talloc_zero(nat, struct bsc_msc_dest);
+	if (!nat->main_dest) {
+		talloc_free(nat);
+		return NULL;
+	}
+
 	INIT_LLIST_HEAD(&nat->sccp_connections);
 	INIT_LLIST_HEAD(&nat->bsc_connections);
+	INIT_LLIST_HEAD(&nat->paging_groups);
 	INIT_LLIST_HEAD(&nat->bsc_configs);
 	INIT_LLIST_HEAD(&nat->access_lists);
-
-	nat->stats.sccp.conn = counter_alloc("nat.sccp.conn");
-	nat->stats.sccp.calls = counter_alloc("nat.sccp.calls");
-	nat->stats.bsc.reconn = counter_alloc("nat.bsc.conn");
-	nat->stats.bsc.auth_fail = counter_alloc("nat.bsc.auth_fail");
-	nat->stats.msc.reconn = counter_alloc("nat.msc.conn");
-	nat->stats.ussd.reconn = counter_alloc("nat.ussd.conn");
-	nat->msc_ip = talloc_strdup(nat, "127.0.0.1");
-	nat->msc_port = 5000;
+	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");
+	nat->stats.bsc.reconn = osmo_counter_alloc("nat.bsc.conn");
+	nat->stats.bsc.auth_fail = osmo_counter_alloc("nat.bsc.auth_fail");
+	nat->stats.msc.reconn = osmo_counter_alloc("nat.msc.conn");
+	nat->stats.ussd.reconn = osmo_counter_alloc("nat.ussd.conn");
 	nat->auth_timeout = 2;
 	nat->ping_timeout = 20;
 	nat->pong_timeout = 5;
+
+	llist_add(&nat->main_dest->list, &nat->dests);
+	nat->main_dest->ip = talloc_strdup(nat, "127.0.0.1");
+	nat->main_dest->port = 5000;
+
 	return nat;
 }
 
 void bsc_nat_set_msc_ip(struct bsc_nat *nat, const char *ip)
 {
-	bsc_replace_string(nat, &nat->msc_ip, ip);
+	bsc_replace_string(nat, &nat->main_dest->ip, ip);
 }
 
 struct bsc_connection *bsc_connection_alloc(struct bsc_nat *nat)
@@ -113,7 +128,7 @@ struct bsc_connection *bsc_connection_alloc(struct bsc_nat *nat)
 		return NULL;
 
 	con->nat = nat;
-	write_queue_init(&con->write_queue, 100);
+	osmo_wqueue_init(&con->write_queue, 100);
 	return con;
 }
 
@@ -127,6 +142,7 @@ struct bsc_config *bsc_config_alloc(struct bsc_nat *nat, const char *token)
 	conf->nr = nat->num_bsc;
 	conf->nat = nat;
 	conf->max_endpoints = 32;
+	conf->paging_group = PAGIN_GROUP_UNASSIGNED;
 
 	INIT_LLIST_HEAD(&conf->lac_list);
 
@@ -147,29 +163,29 @@ void bsc_config_free(struct bsc_config *cfg)
 	rate_ctr_group_free(cfg->stats.ctrg);
 }
 
-void bsc_config_add_lac(struct bsc_config *cfg, int _lac)
+static void _add_lac(void *ctx, struct llist_head *list, int _lac)
 {
 	struct bsc_lac_entry *lac;
 
-	llist_for_each_entry(lac, &cfg->lac_list, entry)
+	llist_for_each_entry(lac, list, entry)
 		if (lac->lac == _lac)
 			return;
 
-	lac = talloc_zero(cfg, struct bsc_lac_entry);
+	lac = talloc_zero(ctx, struct bsc_lac_entry);
 	if (!lac) {
 		LOGP(DNAT, LOGL_ERROR, "Failed to allocate.\n");
 		return;
 	}
 
 	lac->lac = _lac;
-	llist_add_tail(&lac->entry, &cfg->lac_list);
+	llist_add_tail(&lac->entry, list);
 }
 
-void bsc_config_del_lac(struct bsc_config *cfg, int _lac)
+static void _del_lac(struct llist_head *list, int _lac)
 {
 	struct bsc_lac_entry *lac;
 
-	llist_for_each_entry(lac, &cfg->lac_list, entry)
+	llist_for_each_entry(lac, list, entry)
 		if (lac->lac == _lac) {
 			llist_del(&lac->entry);
 			talloc_free(lac);
@@ -177,14 +193,77 @@ void bsc_config_del_lac(struct bsc_config *cfg, int _lac)
 		}
 }
 
+void bsc_config_add_lac(struct bsc_config *cfg, int _lac)
+{
+	_add_lac(cfg, &cfg->lac_list, _lac);
+}
+
+void bsc_config_del_lac(struct bsc_config *cfg, int _lac)
+{
+	_del_lac(&cfg->lac_list, _lac);
+}
+
+struct bsc_nat_paging_group *bsc_nat_paging_group_create(struct bsc_nat *nat, int group)
+{
+	struct bsc_nat_paging_group *pgroup;
+
+	pgroup = talloc_zero(nat, struct bsc_nat_paging_group);
+	if (!pgroup) {
+		LOGP(DNAT, LOGL_ERROR, "Failed to allocate a paging group.\n");
+		return NULL;
+	}
+
+	pgroup->nr = group;
+	INIT_LLIST_HEAD(&pgroup->lists);
+	llist_add_tail(&pgroup->entry, &nat->paging_groups);
+	return pgroup;
+}
+
+void bsc_nat_paging_group_delete(struct bsc_nat_paging_group *pgroup)
+{
+	llist_del(&pgroup->entry);
+	talloc_free(pgroup);
+}
+
+struct bsc_nat_paging_group *bsc_nat_paging_group_num(struct bsc_nat *nat, int group)
+{
+	struct bsc_nat_paging_group *pgroup;
+
+	llist_for_each_entry(pgroup, &nat->paging_groups, entry)
+		if (pgroup->nr == group)
+			return pgroup;
+
+	return NULL;
+}
+
+void bsc_nat_paging_group_add_lac(struct bsc_nat_paging_group *pgroup, int lac)
+{
+	_add_lac(pgroup, &pgroup->lists, lac);
+}
+
+void bsc_nat_paging_group_del_lac(struct bsc_nat_paging_group *pgroup, int lac)
+{
+	_del_lac(&pgroup->lists, lac);
+}
+
 int bsc_config_handles_lac(struct bsc_config *cfg, int lac_nr)
 {
+	struct bsc_nat_paging_group *pgroup;
 	struct bsc_lac_entry *entry;
 
 	llist_for_each_entry(entry, &cfg->lac_list, entry)
 		if (entry->lac == lac_nr)
 			return 1;
 
+	/* now lookup the paging group */
+	pgroup = bsc_nat_paging_group_num(cfg->nat, cfg->paging_group);
+	if (!pgroup)
+		return 0;
+
+	llist_for_each_entry(entry, &pgroup->lists, entry)
+		if (entry->lac == lac_nr)
+			return 1;
+
 	return 0;
 }
 
@@ -198,25 +277,23 @@ void sccp_connection_destroy(struct sccp_connections *conn)
 	talloc_free(conn);
 }
 
-struct bsc_connection *bsc_nat_find_bsc(struct bsc_nat *nat, struct msgb *msg, int *lac_out)
+
+int bsc_nat_find_paging(struct msgb *msg,
+			const uint8_t **out_data, int *out_leng)
 {
-	struct bsc_connection *bsc;
 	int data_length;
 	const uint8_t *data;
 	struct tlv_parsed tp;
-	int i = 0;
-
-	*lac_out = -1;
 
 	if (!msg->l3h || msgb_l3len(msg) < 3) {
 		LOGP(DNAT, LOGL_ERROR, "Paging message is too short.\n");
-		return NULL;
+		return -1;
 	}
 
 	tlv_parse(&tp, gsm0808_att_tlvdef(), msg->l3h + 3, msgb_l3len(msg) - 3, 0, 0);
 	if (!TLVP_PRESENT(&tp, GSM0808_IE_CELL_IDENTIFIER_LIST)) {
 		LOGP(DNAT, LOGL_ERROR, "No CellIdentifier List inside paging msg.\n");
-		return NULL;
+		return -2;
 	}
 
 	data_length = TLVP_LEN(&tp, GSM0808_IE_CELL_IDENTIFIER_LIST);
@@ -224,29 +301,15 @@ struct bsc_connection *bsc_nat_find_bsc(struct bsc_nat *nat, struct msgb *msg, i
 
 	/* No need to try a different BSS */
 	if (data[0] == CELL_IDENT_BSS) {
-		return NULL;
+		return -3;
 	} else if (data[0] != CELL_IDENT_LAC) {
 		LOGP(DNAT, LOGL_ERROR, "Unhandled cell ident discrminator: %d\n", data[0]);
-		return NULL;
-	}
-
-	/* Currently we only handle one BSC */
-	for (i = 1; i < data_length - 1; i += 2) {
-		unsigned int _lac = ntohs(*(unsigned int *) &data[i]);
-		*lac_out = _lac;
-		llist_for_each_entry(bsc, &nat->bsc_connections, list_entry) {
-			if (!bsc->cfg)
-				continue;
-			if (!bsc->authenticated)
-				continue;
-			if (!bsc_config_handles_lac(bsc->cfg, _lac))
-				continue;
-
-			return bsc;
-		}
+		return -4;
 	}
 
-	return NULL;
+	*out_data = &data[1];
+	*out_leng = data_length - 1;
+	return 0;
 }
 
 int bsc_write_mgcp(struct bsc_connection *bsc, const uint8_t *data, unsigned int length)
@@ -276,16 +339,16 @@ int bsc_write(struct bsc_connection *bsc, struct msgb *msg, int proto)
 	return bsc_do_write(&bsc->write_queue, msg, proto);
 }
 
-int bsc_do_write(struct write_queue *queue, struct msgb *msg, int proto)
+int bsc_do_write(struct osmo_wqueue *queue, struct msgb *msg, int proto)
 {
 	/* prepend the header */
 	ipaccess_prepend_header(msg, proto);
 	return bsc_write_msg(queue, msg);
 }
 
-int bsc_write_msg(struct write_queue *queue, struct msgb *msg)
+int bsc_write_msg(struct osmo_wqueue *queue, struct msgb *msg)
 {
-	if (write_queue_enqueue(queue, msg) != 0) {
+	if (osmo_wqueue_enqueue(queue, msg) != 0) {
 		LOGP(DINP, LOGL_ERROR, "Failed to enqueue the write.\n");
 		msgb_free(msg);
 		return -1;
@@ -500,7 +563,7 @@ int bsc_nat_filter_sccp_cr(struct bsc_connection *bsc, struct msgb *msg,
 	struct gsm48_hdr *hdr48;
 	int hdr48_len;
 	int len;
-	uint8_t msg_type;
+	uint8_t msg_type, proto;
 
 	*con_type = NAT_CON_TYPE_NONE;
 	*imsi = NULL;
@@ -538,18 +601,19 @@ int bsc_nat_filter_sccp_cr(struct bsc_connection *bsc, struct msgb *msg,
 
 	hdr48 = (struct gsm48_hdr *) TLVP_VAL(&tp, GSM0808_IE_LAYER_3_INFORMATION);
 
+	proto = hdr48->proto_discr & 0x0f;
 	msg_type = hdr48->msg_type & 0xbf;
-	if (hdr48->proto_discr == GSM48_PDISC_MM &&
+	if (proto == GSM48_PDISC_MM &&
 	    msg_type == GSM48_MT_MM_LOC_UPD_REQUEST) {
 		*con_type = NAT_CON_TYPE_LU;
 		return _cr_check_loc_upd(bsc, &hdr48->data[0], hdr48_len - sizeof(*hdr48), imsi);
-	} else if (hdr48->proto_discr == GSM48_PDISC_MM &&
+	} else if (proto == GSM48_PDISC_MM &&
 		  msg_type == GSM48_MT_MM_CM_SERV_REQ) {
 		*con_type = NAT_CON_TYPE_CM_SERV_REQ;
 		return _cr_check_cm_serv_req(bsc, &hdr48->data[0],
 					     hdr48_len - sizeof(*hdr48),
 					     con_type, imsi);
-	} else if (hdr48->proto_discr == GSM48_PDISC_RR &&
+	} else if (proto == GSM48_PDISC_RR &&
 		   msg_type == GSM48_MT_RR_PAG_RESP) {
 		*con_type = NAT_CON_TYPE_PAG_RESP;
 		return _cr_check_pag_resp(bsc, &hdr48->data[0], hdr48_len - sizeof(*hdr48), imsi);
@@ -583,7 +647,7 @@ int bsc_nat_filter_dt(struct bsc_connection *bsc, struct msgb *msg,
 		      struct sccp_connections *con, struct bsc_nat_parsed *parsed)
 {
 	uint32_t len;
-	uint8_t msg_type;
+	uint8_t msg_type, proto;
 	struct gsm48_hdr *hdr48;
 
 	if (con->imsi_checked)
@@ -597,8 +661,9 @@ int bsc_nat_filter_dt(struct bsc_connection *bsc, struct msgb *msg,
 	if (!hdr48)
 		return -1;
 
+	proto = hdr48->proto_discr & 0x0f;
 	msg_type = hdr48->msg_type & 0xbf;
-	if (hdr48->proto_discr == GSM48_PDISC_MM &&
+	if (proto == GSM48_PDISC_MM &&
 	    msg_type == GSM48_MT_MM_ID_RESP) {
 		return _dt_check_id_resp(bsc, &hdr48->data[0], len - sizeof(*hdr48), con);
 	} else {
@@ -606,8 +671,11 @@ int bsc_nat_filter_dt(struct bsc_connection *bsc, struct msgb *msg,
 	}
 }
 
-void bsc_parse_reg(void *ctx, regex_t *reg, char **imsi, int argc, const char **argv)
+int bsc_parse_reg(void *ctx, regex_t *reg, char **imsi, int argc, const char **argv)
 {
+	int ret;
+
+	ret = 0;
 	if (*imsi) {
 		talloc_free(*imsi);
 		*imsi = NULL;
@@ -616,8 +684,16 @@ void bsc_parse_reg(void *ctx, regex_t *reg, char **imsi, int argc, const char **
 
 	if (argc > 0) {
 		*imsi = talloc_strdup(ctx, argv[0]);
-		regcomp(reg, argv[0], 0);
+		ret = regcomp(reg, argv[0], 0);
+
+		/* handle compilation failures */
+		if (ret != 0) {
+			talloc_free(*imsi);
+			*imsi = NULL;
+		}
 	}
+
+	return ret;
 }
 
 static const char *con_types [] = {
@@ -715,7 +791,7 @@ int bsc_conn_type_to_ctr(struct sccp_connections *conn)
 	return con_to_ctr[conn->con_type];
 }
 
-int bsc_write_cb(struct bsc_fd *bfd, struct msgb *msg)
+int bsc_write_cb(struct osmo_fd *bfd, struct msgb *msg)
 {
 	int rc;
 
@@ -726,109 +802,82 @@ int bsc_write_cb(struct bsc_fd *bfd, struct msgb *msg)
 	return rc;
 }
 
+static char *rewrite_non_international(struct bsc_nat *nat, void *ctx, const char *imsi,
+				       struct gsm_mncc_number *called)
+{
+	struct bsc_nat_num_rewr_entry *entry;
+	char *new_number = NULL;
+
+	if (llist_empty(&nat->num_rewr))
+		return NULL;
+
+	if (called->plan != 1)
+		return NULL;
+	if (called->type == 1)
+		return NULL;
+
+	/* need to find a replacement and then fix it */
+	llist_for_each_entry(entry, &nat->num_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, called->number, 2, matches, 0) == 0 &&
+		    matches[1].rm_eo != -1)
+			new_number = talloc_asprintf(ctx, "%s%s",
+					entry->replace,
+					&called->number[matches[1].rm_so]);
+		if (new_number)
+			break;
+	}
+
+	return new_number;
+}
+
+
 /**
  * Rewrite non global numbers... according to rules based on the IMSI
  */
-struct msgb *bsc_nat_rewrite_setup(struct bsc_nat *nat, struct msgb *msg, struct bsc_nat_parsed *parsed, const char *imsi)
+static struct msgb *rewrite_setup(struct bsc_nat *nat, struct msgb *msg,
+				  struct bsc_nat_parsed *parsed, const char *imsi,
+				  struct gsm48_hdr *hdr48, const uint32_t len)
 {
 	struct tlv_parsed tp;
-	struct gsm48_hdr *hdr48;
-	uint32_t len;
-	uint8_t msg_type;
 	unsigned int payload_len;
 	struct gsm_mncc_number called;
-	struct msg_entry *entry;
+	struct msgb *out;
 	char *new_number = NULL;
-	struct msgb *out, *sccp;
 	uint8_t *outptr;
 	const uint8_t *msgptr;
 	int sec_len;
 
-	if (!imsi || strlen(imsi) < 5)
-		return msg;
-
-	if (!nat->num_rewr)
-		return msg;
-
-	/* only care about DTAP messages */
-	if (parsed->bssap != BSSAP_MSG_DTAP)
-		return msg;
-	if (!parsed->dest_local_ref)
-		return msg;
-
-	hdr48 = bsc_unpack_dtap(parsed, msg, &len);
-	if (!hdr48)
-		return msg;
-
-	msg_type = hdr48->msg_type & 0xbf;
-	if (hdr48->proto_discr != GSM48_PDISC_CC ||
-	    msg_type != GSM48_MT_CC_SETUP)
-		return msg;
-
 	/* decode and rewrite the message */
 	payload_len = len - sizeof(*hdr48);
 	tlv_parse(&tp, &gsm48_att_tlvdef, hdr48->data, payload_len, 0, 0);
 
 	/* no number, well let us ignore it */
 	if (!TLVP_PRESENT(&tp, GSM48_IE_CALLED_BCD))
-		return msg;
+		return NULL;
 
 	memset(&called, 0, sizeof(called));
 	gsm48_decode_called(&called,
 			    TLVP_VAL(&tp, GSM48_IE_CALLED_BCD) - 1);
 
 	/* check if it looks international and stop */
-	if (called.plan != 1)
-		return msg;
-	if (called.type == 1)
-		return msg;
-	if (strncmp(called.number, "00", 2) == 0)
-		return msg;
-
-	/* need to find a replacement and then fix it */
-	llist_for_each_entry(entry, &nat->num_rewr->entry, list) {
-		regex_t reg;
-		regmatch_t matches[2];
-
-		if (entry->mcc[0] != '*' && strncmp(entry->mcc, imsi, 3) != 0)
-			continue;
-		if (entry->mnc[0] != '*' && strncmp(entry->mnc, imsi + 3, 2) != 0)
-			continue;
-
-		if (entry->text[0] == '+') {
-			LOGP(DNAT, LOGL_ERROR,
-				"Plus is not allowed in the number");
-			continue;
-		}
-
-		/* We have an entry for the IMSI. Need to match now */
-		if (regcomp(&reg, entry->option, REG_EXTENDED) != 0) {
-			LOGP(DNAT, LOGL_ERROR,
-				"Regexp '%s' is not valid.\n", entry->option);
-			continue;
-		}
-
-		/* this regexp matches... */
-		if (regexec(&reg, called.number, 2, matches, 0) == 0 &&
-		    matches[1].rm_eo != -1)
-			new_number = talloc_asprintf(msg, "%s%s",
-					entry->text,
-					&called.number[matches[1].rm_so]);
-		regfree(&reg);
-
-		if (new_number)
-			break;
-	}
+	new_number = rewrite_non_international(nat, msg, imsi, &called);
 
 	if (!new_number) {
 		LOGP(DNAT, LOGL_DEBUG, "No IMSI match found, returning message.\n");
-		return msg;
+		return NULL;
 	}
 
 	if (strlen(new_number) > sizeof(called.number)) {
 		LOGP(DNAT, LOGL_ERROR, "Number is too long for structure.\n");
 		talloc_free(new_number);
-		return msg;
+		return NULL;
 	}
 
 	/*
@@ -841,7 +890,7 @@ struct msgb *bsc_nat_rewrite_setup(struct bsc_nat *nat, struct msgb *msg, struct
 	if (!out) {
 		LOGP(DNAT, LOGL_ERROR, "Failed to allocate.\n");
 		talloc_free(new_number);
-		return msg;
+		return NULL;
 	}
 
 	/* copy the header */
@@ -869,25 +918,324 @@ struct msgb *bsc_nat_rewrite_setup(struct bsc_nat *nat, struct msgb *msg, struct
 	outptr = msgb_put(out, sec_len);
 	memcpy(outptr, msgptr, sec_len);
 
+	talloc_free(new_number);
+	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];
+
+
+	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;
+	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;
+	}
+
+	/* 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];
+
+		/* 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;
+
+	/*
+	 * 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.
+	 */
+	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;
+	uint32_t len;
+	uint8_t msg_type, proto;
+	struct msgb *new_msg = NULL, *sccp;
+
+	if (!imsi || strlen(imsi) < 5)
+		return msg;
+
+	/* only care about DTAP messages */
+	if (parsed->bssap != BSSAP_MSG_DTAP)
+		return msg;
+	if (!parsed->dest_local_ref)
+		return msg;
+
+	hdr48 = bsc_unpack_dtap(parsed, msg, &len);
+	if (!hdr48)
+		return msg;
+
+	proto = hdr48->proto_discr & 0x0f;
+	msg_type = hdr48->msg_type & 0xbf;
+
+	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;
+
 	/* wrap with DTAP, SCCP, then IPA. TODO: Stop copying */
-	gsm0808_prepend_dtap_header(out, 0);
-	sccp = sccp_create_dt1(parsed->dest_local_ref, out->data, out->len);
+	gsm0808_prepend_dtap_header(new_msg, 0);
+	sccp = sccp_create_dt1(parsed->dest_local_ref, new_msg->data, new_msg->len);
+	talloc_free(new_msg);
+
 	if (!sccp) {
 		LOGP(DNAT, LOGL_ERROR, "Failed to allocate.\n");
-		talloc_free(new_number);
-		talloc_free(out);
 		return msg;
 	}
 
 	ipaccess_prepend_header(sccp, IPAC_PROTO_SCCP);
 
-	/* give up memory, we are done */
-	talloc_free(new_number);
 	/* the parsed hangs off from msg but it needs to survive */
 	talloc_steal(sccp, parsed);
 	msgb_free(msg);
-	msgb_free(out);
-	out = NULL;
 	return sccp;
 }
 
+static void num_rewr_free_data(struct bsc_nat_num_rewr_entry *entry)
+{
+	regfree(&entry->msisdn_reg);
+	regfree(&entry->num_reg);
+	talloc_free(entry->replace);
+}
+
+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, head, list) {
+		num_rewr_free_data(entry);
+		llist_del(&entry->list);
+		talloc_free(entry);
+	}
+
+
+	if (!list)
+		return;
+
+	llist_for_each_entry(cfg_entry, &list->entry, list) {
+		char *regexp;
+		if (cfg_entry->text[0] == '+') {
+			LOGP(DNAT, LOGL_ERROR,
+				"Plus is not allowed in the number\n");
+			continue;
+		}
+
+		entry = talloc_zero(ctx, struct bsc_nat_num_rewr_entry);
+		if (!entry) {
+			LOGP(DNAT, LOGL_ERROR,
+				"Allication of the num_rewr entry failed.\n");
+			continue;
+		}
+
+		entry->replace = talloc_strdup(entry, cfg_entry->text);
+		if (!entry->replace) {
+			LOGP(DNAT, LOGL_ERROR,
+				"Failed to copy the replacement text.\n");
+			talloc_free(entry);
+			continue;
+		}
+
+		/* we will now build a regexp string */
+		if (cfg_entry->mcc[0] == '^') {
+			regexp = talloc_strdup(entry, cfg_entry->mcc);
+		} else {
+			regexp = talloc_asprintf(entry, "^%s%s",
+					cfg_entry->mcc[0] == '*' ?
+						"[0-9][0-9][0-9]" : cfg_entry->mcc,
+					cfg_entry->mnc[0] == '*' ?
+						"[0-9][0-9]" : cfg_entry->mnc);
+		}
+
+		if (!regexp) {
+			LOGP(DNAT, LOGL_ERROR, "Failed to create a regexp string.\n");
+			talloc_free(entry);
+			continue;
+		}
+
+		if (regcomp(&entry->msisdn_reg, regexp, 0) != 0) {
+			LOGP(DNAT, LOGL_ERROR,
+				"Failed to compile regexp '%s'\n", regexp);
+			talloc_free(regexp);
+			talloc_free(entry);
+			continue;
+		}
+
+		talloc_free(regexp);
+		if (regcomp(&entry->num_reg, cfg_entry->option, REG_EXTENDED) != 0) {
+			LOGP(DNAT, LOGL_ERROR,
+				"Failed to compile regexp '%s\n'", cfg_entry->option);
+			regfree(&entry->msisdn_reg);
+			talloc_free(entry);
+			continue;
+		}
+
+		/* we have copied the number */
+		llist_add_tail(&entry->list, head);
+	}
+}
diff --git a/src/osmo-bsc_nat/bsc_nat_vty.c b/src/osmo-bsc_nat/bsc_nat_vty.c
index 786db2dc2..b5c1cf287 100644
--- a/src/osmo-bsc_nat/bsc_nat_vty.c
+++ b/src/osmo-bsc_nat/bsc_nat_vty.c
@@ -1,6 +1,6 @@
 /* OpenBSC NAT interface to quagga VTY */
-/* (C) 2010 by Holger Hans Peter Freyther
- * (C) 2010 by On-Waves
+/* (C) 2010-2011 by Holger Hans Peter Freyther
+ * (C) 2010-2011 by On-Waves
  * All Rights Reserved
  *
  * This program is free software; you can redistribute it and/or modify
@@ -26,9 +26,11 @@
 #include <openbsc/mgcp.h>
 #include <openbsc/vty.h>
 
-#include <osmocore/talloc.h>
-#include <osmocore/rate_ctr.h>
-#include <osmocore/utils.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/rate_ctr.h>
+#include <osmocom/core/utils.h>
+#include <osmocom/vty/logging.h>
+#include <osmocom/vty/misc.h>
 
 #include <osmocom/sccp/sccp.h>
 
@@ -36,6 +38,10 @@
 
 static struct bsc_nat *_nat;
 
+
+#define PAGING_STR "Paging\n"
+#define SMSC_REWRITE "SMSC Rewriting\n"
+
 static struct cmd_node nat_node = {
 	NAT_NODE,
 	"%s(nat)#",
@@ -48,6 +54,17 @@ static struct cmd_node bsc_node = {
 	1,
 };
 
+static struct cmd_node pgroup_node = {
+	PGROUP_NODE,
+	"%s(paging-group)#",
+	1,
+};
+
+static int config_write_pgroup(struct vty *vty)
+{
+	return CMD_SUCCESS;
+}
+
 static void write_acc_lst(struct vty *vty, struct bsc_nat_acc_lst *lst)
 {
 	struct bsc_nat_acc_lst_entry *entry;
@@ -62,13 +79,28 @@ static void write_acc_lst(struct vty *vty, struct bsc_nat_acc_lst *lst)
 	}
 }
 
+static void dump_lac(struct vty *vty, struct llist_head *head)
+{
+	struct bsc_lac_entry *lac;
+	llist_for_each_entry(lac, head, entry)
+		vty_out(vty, "  location_area_code %u%s", lac->lac, VTY_NEWLINE);
+}
+
+
+static void write_pgroup_lst(struct vty *vty, struct bsc_nat_paging_group *pgroup)
+{
+	vty_out(vty, " paging-group %d%s", pgroup->nr, VTY_NEWLINE);
+	dump_lac(vty, &pgroup->lists);
+}
+
 static int config_write_nat(struct vty *vty)
 {
 	struct bsc_nat_acc_lst *lst;
+	struct bsc_nat_paging_group *pgroup;
 
 	vty_out(vty, "nat%s", VTY_NEWLINE);
-	vty_out(vty, " msc ip %s%s", _nat->msc_ip, VTY_NEWLINE);
-	vty_out(vty, " msc port %d%s", _nat->msc_port, VTY_NEWLINE);
+	vty_out(vty, " msc ip %s%s", _nat->main_dest->ip, VTY_NEWLINE);
+	vty_out(vty, " msc port %d%s", _nat->main_dest->port, VTY_NEWLINE);
 	vty_out(vty, " timeout auth %d%s", _nat->auth_timeout, VTY_NEWLINE);
 	vty_out(vty, " timeout ping %d%s", _nat->ping_timeout, VTY_NEWLINE);
 	vty_out(vty, " timeout pong %d%s", _nat->pong_timeout, VTY_NEWLINE);
@@ -88,32 +120,34 @@ static int config_write_nat(struct vty *vty)
 
 	if (_nat->num_rewr_name)
 		vty_out(vty, " number-rewrite %s%s", _nat->num_rewr_name, VTY_NEWLINE);
-
-	llist_for_each_entry(lst, &_nat->access_lists, list) {
+	if (_nat->smsc_rewr_name)
+		vty_out(vty, " rewrite-smsc addr %s%s",
+			_nat->smsc_rewr_name, VTY_NEWLINE);
+	if (_nat->tpdest_match_name)
+		vty_out(vty, " rewrite-smsc tp-dest-match %s%s",
+			_nat->tpdest_match_name, VTY_NEWLINE);
+
+	llist_for_each_entry(lst, &_nat->access_lists, list)
 		write_acc_lst(vty, lst);
-	}
+	llist_for_each_entry(pgroup, &_nat->paging_groups, entry)
+		write_pgroup_lst(vty, pgroup);
 
 	return CMD_SUCCESS;
 }
 
-static void dump_lac(struct vty *vty, struct bsc_config *cfg)
-{
-	struct bsc_lac_entry *lac;
-	llist_for_each_entry(lac, &cfg->lac_list, entry)
-		vty_out(vty, "  location_area_code %u%s", lac->lac, VTY_NEWLINE);
-}
-
 static void config_write_bsc_single(struct vty *vty, struct bsc_config *bsc)
 {
 	vty_out(vty, " bsc %u%s", bsc->nr, VTY_NEWLINE);
 	vty_out(vty, "  token %s%s", bsc->token, VTY_NEWLINE);
-	dump_lac(vty, bsc);
-	vty_out(vty, "  paging forbidden %d%s", bsc->forbid_paging, VTY_NEWLINE);
+	dump_lac(vty, &bsc->lac_list);
 	if (bsc->description)
 		vty_out(vty, "  description %s%s", bsc->description, VTY_NEWLINE);
 	if (bsc->acc_lst_name)
 		vty_out(vty, "  access-list-name %s%s", bsc->acc_lst_name, VTY_NEWLINE);
 	vty_out(vty, "  max-endpoints %d%s", bsc->max_endpoints, VTY_NEWLINE);
+	if (bsc->paging_group != -1)
+		vty_out(vty, "  paging group %d%s", bsc->paging_group, VTY_NEWLINE);
+	vty_out(vty, "  paging forbidden %d%s", bsc->forbid_paging, VTY_NEWLINE);
 }
 
 static int config_write_bsc(struct vty *vty)
@@ -226,15 +260,15 @@ static void dump_stat_total(struct vty *vty, struct bsc_nat *nat)
 {
 	vty_out(vty, "NAT statistics%s", VTY_NEWLINE);
 	vty_out(vty, " SCCP Connections %lu total, %lu calls%s",
-		counter_get(nat->stats.sccp.conn),
-		counter_get(nat->stats.sccp.calls), VTY_NEWLINE);
+		osmo_counter_get(nat->stats.sccp.conn),
+		osmo_counter_get(nat->stats.sccp.calls), VTY_NEWLINE);
 	vty_out(vty, " MSC Connections %lu%s",
-		counter_get(nat->stats.msc.reconn), VTY_NEWLINE);
+		osmo_counter_get(nat->stats.msc.reconn), VTY_NEWLINE);
 	vty_out(vty, " MSC Connected: %d%s",
 		nat->msc_con->is_connected, VTY_NEWLINE);
 	vty_out(vty, " BSC Connections %lu total, %lu auth failed.%s",
-		counter_get(nat->stats.bsc.reconn),
-		counter_get(nat->stats.bsc.auth_fail), VTY_NEWLINE);
+		osmo_counter_get(nat->stats.bsc.reconn),
+		osmo_counter_get(nat->stats.bsc.auth_fail), VTY_NEWLINE);
 }
 
 static void dump_stat_bsc(struct vty *vty, struct bsc_config *conf)
@@ -309,8 +343,7 @@ DEFUN(show_msc,
 		return CMD_WARNING;
 	}
 
-	vty_out(vty, "MSC on %s:%d is connected: %d%s\n",
-		_nat->msc_con->ip, _nat->msc_con->port,
+	vty_out(vty, "MSC is connected: %d%s\n",
 		_nat->msc_con->is_connected, VTY_NEWLINE);
 	return CMD_SUCCESS;
 }
@@ -355,7 +388,7 @@ DEFUN(cfg_nat_msc_port,
       "msc port <1-65500>",
       "Set the port of the MSC.")
 {
-	_nat->msc_port = atoi(argv[0]);
+	_nat->main_dest->port = atoi(argv[0]);
 	return CMD_SUCCESS;
 }
 
@@ -417,23 +450,65 @@ DEFUN(cfg_nat_acc_lst_name,
 	return CMD_SUCCESS;
 }
 
+DEFUN(cfg_nat_no_acc_lst_name,
+      cfg_nat_no_acc_lst_name_cmd,
+      "no access-list-name",
+      NO_STR "Remove the access list from the NAT.\n")
+{
+	if (_nat->acc_lst_name) {
+		talloc_free(_nat->acc_lst_name);
+		_nat->acc_lst_name = NULL;
+	}
+
+	return CMD_SUCCESS;
+}
+
+static int replace_rules(struct bsc_nat *nat, char **name,
+			 struct llist_head *head, const char *file)
+{
+	struct osmo_config_list *rewr = NULL;
+
+	bsc_replace_string(nat, name, file);
+	if (*name) {
+		rewr = osmo_config_list_parse(nat, *name);
+		bsc_nat_num_rewr_entry_adapt(nat, head, rewr);
+		talloc_free(rewr);
+		return CMD_SUCCESS;
+	} else {
+		bsc_nat_num_rewr_entry_adapt(nat, head, NULL);
+		return CMD_SUCCESS;
+	}
+}
+
 DEFUN(cfg_nat_number_rewrite,
       cfg_nat_number_rewrite_cmd,
       "number-rewrite FILENAME",
       "Set the file with rewriting rules.\n" "Filename")
 {
-	bsc_replace_string(_nat, &_nat->num_rewr_name, argv[0]);
-	if (_nat->num_rewr_name) {
-		if (_nat->num_rewr)
-			talloc_free(_nat->num_rewr);
-		_nat->num_rewr = msg_entry_parse(_nat, _nat->num_rewr_name);
-		return _nat->num_rewr == NULL ? CMD_WARNING : CMD_SUCCESS;
-	} else {
-		if (_nat->num_rewr)
-			talloc_free(_nat->num_rewr);
-		_nat->num_rewr = NULL;
-		return CMD_SUCCESS;
-	}
+	return replace_rules(_nat, &_nat->num_rewr_name,
+			     &_nat->num_rewr, argv[0]);
+}
+
+DEFUN(cfg_nat_smsc_addr,
+      cfg_nat_smsc_addr_cmd,
+      "rewrite-smsc addr FILENAME",
+      SMSC_REWRITE
+      "The SMSC Address to match and replace in RP-DATA\n"
+      "File with rules for the SMSC Address replacing\n")
+{
+	return replace_rules(_nat, &_nat->smsc_rewr_name,
+			     &_nat->smsc_rewr, argv[0]);
+}
+
+DEFUN(cfg_nat_smsc_tpdest,
+      cfg_nat_smsc_tpdest_cmd,
+      "rewrite-smsc tp-dest-match FILENAME",
+      SMSC_REWRITE
+      "Match TP-Destination of a SMS.\n"
+      "File with rules for matching MSISDN and TP-DEST\n")
+{
+	return replace_rules(_nat, &_nat->tpdest_match_name,
+			     &_nat->tpdest_match, argv[0]);
 }
 
 DEFUN(cfg_nat_ussd_lst_name,
@@ -448,11 +523,12 @@ DEFUN(cfg_nat_ussd_lst_name,
 
 DEFUN(cfg_nat_ussd_query,
       cfg_nat_ussd_query_cmd,
-      "ussd-query QUERY",
+      "ussd-query REGEXP",
       "Set the USSD query to match with the ussd-list-name\n"
       "The query to match")
 {
-	bsc_replace_string(_nat, &_nat->ussd_query, argv[0]);
+	if (bsc_parse_reg(_nat, &_nat->ussd_query_re, &_nat->ussd_query, argc, argv) != 0)
+		return CMD_WARNING;
 	return CMD_SUCCESS;
 }
 
@@ -508,7 +584,7 @@ DEFUN(cfg_bsc_token, cfg_bsc_token_cmd, "token TOKEN", "Set the token")
 }
 
 DEFUN(cfg_bsc_lac, cfg_bsc_lac_cmd, "location_area_code <0-65535>",
-      "Set the Location Area Code (LAC) of this BSC")
+      "Add the Location Area Code (LAC) of this BSC\n" "LAC\n")
 {
 	struct bsc_config *tmp;
 	struct bsc_config *conf = vty->index;
@@ -536,7 +612,7 @@ DEFUN(cfg_bsc_lac, cfg_bsc_lac_cmd, "location_area_code <0-65535>",
 
 DEFUN(cfg_bsc_no_lac, cfg_bsc_no_lac_cmd,
       "no location_area_code <0-65535>",
-      NO_STR "Set the Location Area Code (LAC) of this BSC")
+      NO_STR "Remove the Location Area Code (LAC) of this BSC\n" "LAC\n")
 {
 	int lac = atoi(argv[0]);
 	struct bsc_config *conf = vty->index;
@@ -550,7 +626,7 @@ DEFUN(cfg_bsc_no_lac, cfg_bsc_no_lac_cmd,
 DEFUN(cfg_lst_imsi_allow,
       cfg_lst_imsi_allow_cmd,
       "access-list NAME imsi-allow [REGEXP]",
-      "Allow IMSIs matching the REGEXP\n"
+      "Add the regexp to the allowed list\n"
       "The name of the access-list\n"
       "The regexp of allowed IMSIs\n")
 {
@@ -565,14 +641,15 @@ DEFUN(cfg_lst_imsi_allow,
 	if (!entry)
 		return CMD_WARNING;
 
-	bsc_parse_reg(acc, &entry->imsi_allow_re, &entry->imsi_allow, argc - 1, &argv[1]);
+	if (bsc_parse_reg(acc, &entry->imsi_allow_re, &entry->imsi_allow, argc - 1, &argv[1]) != 0)
+		return CMD_WARNING;
 	return CMD_SUCCESS;
 }
 
 DEFUN(cfg_lst_imsi_deny,
       cfg_lst_imsi_deny_cmd,
       "access-list NAME imsi-deny [REGEXP]",
-      "Allow IMSIs matching the REGEXP\n"
+      "Add the regexp to the deny list\n"
       "The name of the access-list\n"
       "The regexp of to be denied IMSIs\n")
 {
@@ -587,7 +664,8 @@ DEFUN(cfg_lst_imsi_deny,
 	if (!entry)
 		return CMD_WARNING;
 
-	bsc_parse_reg(acc, &entry->imsi_deny_re, &entry->imsi_deny, argc - 1, &argv[1]);
+	if (bsc_parse_reg(acc, &entry->imsi_deny_re, &entry->imsi_deny, argc - 1, &argv[1]) != 0)
+		return CMD_WARNING;
 	return CMD_SUCCESS;
 }
 
@@ -636,6 +714,21 @@ DEFUN(cfg_bsc_acc_lst_name,
 	return CMD_SUCCESS;
 }
 
+DEFUN(cfg_bsc_no_acc_lst_name,
+      cfg_bsc_no_acc_lst_name_cmd,
+      "no access-list-name",
+      NO_STR "Do not use an access-list for the BSC.\n")
+{
+	struct bsc_config *conf = vty->index;
+
+	if (conf->acc_lst_name) {
+		talloc_free(conf->acc_lst_name);
+		conf->acc_lst_name = NULL;
+	}
+
+	return CMD_SUCCESS;
+}
+
 DEFUN(cfg_bsc_max_endps, cfg_bsc_max_endps_cmd,
       "max-endpoints <1-1024>",
       "Highest endpoint to use (exclusively)\n" "Number of ports\n")
@@ -649,7 +742,7 @@ DEFUN(cfg_bsc_max_endps, cfg_bsc_max_endps_cmd,
 DEFUN(cfg_bsc_paging,
       cfg_bsc_paging_cmd,
       "paging forbidden (0|1)",
-      "Forbid sending PAGING REQUESTS to the BSC.")
+      PAGING_STR "Forbid sending PAGING REQUESTS to the BSC.")
 {
 	struct bsc_config *conf = vty->index;
 
@@ -672,6 +765,30 @@ DEFUN(cfg_bsc_desc,
 	return CMD_SUCCESS;
 }
 
+DEFUN(cfg_bsc_paging_grp,
+      cfg_bsc_paging_grp_cmd,
+      "paging group <0-1000>",
+      PAGING_STR "Use a paging group\n" "Paging Group to use\n")
+{
+	struct bsc_config *conf = vty->index;
+	conf->paging_group = atoi(argv[0]);
+	return CMD_SUCCESS;
+}
+
+ALIAS_DEPRECATED(cfg_bsc_paging_grp, cfg_bsc_old_grp_cmd,
+		 "paging-group <0-1000>",
+		 "Use a paging group\n" "Paging Group to use\n")
+
+DEFUN(cfg_bsc_no_paging_grp,
+      cfg_bsc_no_paging_grp_cmd,
+      "no paging group",
+      NO_STR PAGING_STR "Disable the usage of a paging group.\n")
+{
+	struct bsc_config *conf = vty->index;
+	conf->paging_group = PAGIN_GROUP_UNASSIGNED;
+	return CMD_SUCCESS;
+}
+
 DEFUN(test_regex, test_regex_cmd,
       "test regex PATTERN STRING",
       "Check if the string is matching the current pattern.")
@@ -680,7 +797,8 @@ DEFUN(test_regex, test_regex_cmd,
 	char *str = NULL;
 
 	memset(&reg, 0, sizeof(reg));
-	bsc_parse_reg(_nat, &reg, &str, 1, argv);
+	if (bsc_parse_reg(_nat, &reg, &str, 1, argv) != 0)
+		return CMD_WARNING;
 
 	vty_out(vty, "String matches allow pattern: %d%s",
 		regexec(&reg, argv[1], 0, NULL, 0) == 0, VTY_NEWLINE);
@@ -715,6 +833,81 @@ DEFUN(set_last_endp, set_last_endp_cmd,
 	return CMD_SUCCESS;
 }
 
+DEFUN(block_new_conn, block_new_conn_cmd,
+      "nat-block (block|unblock)",
+      "Block the NAT for new connections\n"
+      "Block\n" "Unblock\n")
+{
+	_nat->blocked = argv[0][0] == 'b';
+	vty_out(vty, "%%Going to %s the NAT.%s",
+		_nat->blocked ? "block" : "unblock", VTY_NEWLINE);
+	return CMD_SUCCESS;
+}
+
+/* paging group */
+DEFUN(cfg_nat_pgroup, cfg_nat_pgroup_cmd,
+      "paging-group <0-1000>",
+      "Create a Paging Group\n" "Number of the Group\n")
+{
+	int group = atoi(argv[0]);
+	struct bsc_nat_paging_group *pgroup;
+	pgroup = bsc_nat_paging_group_num(_nat, group);
+	if (!pgroup)
+		pgroup = bsc_nat_paging_group_create(_nat, group);
+	if (!pgroup) {
+		vty_out(vty, "Failed to create the group.%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	vty->index = pgroup;
+	vty->node = PGROUP_NODE;
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_nat_no_pgroup, cfg_nat_no_pgroup_cmd,
+      "no paging-group <0-1000>",
+      NO_STR "Delete paging-group\n")
+{
+	int group = atoi(argv[0]);
+	struct bsc_nat_paging_group *pgroup;
+	pgroup = bsc_nat_paging_group_num(_nat, group);
+	if (!pgroup) {
+		vty_out(vty, "No such paging group %d.%s", group, VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	bsc_nat_paging_group_delete(pgroup);
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_pgroup_lac, cfg_pgroup_lac_cmd,
+      "location_area_code <0-65535>",
+       "Add the Location Area Code (LAC)\n" "LAC\n")
+{
+	struct bsc_nat_paging_group *pgroup = vty->index;
+
+	int lac = atoi(argv[0]);
+	if (lac == GSM_LAC_RESERVED_DETACHED || lac == GSM_LAC_RESERVED_ALL_BTS) {
+		vty_out(vty, "%% LAC %d is reserved by GSM 04.08%s",
+			lac, VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	bsc_nat_paging_group_add_lac(pgroup, lac);
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_pgroup_no_lac, cfg_pgroup_no_lac_cmd,
+      "no location_area_code <0-65535>",
+      NO_STR "Remove the Location Area Code (LAC)\n" "LAC\n")
+{
+	int lac = atoi(argv[0]);
+	struct bsc_nat_paging_group *pgroup = vty->index;
+
+	bsc_nat_paging_group_del_lac(pgroup, lac);
+	return CMD_SUCCESS;
+}
+
 int bsc_nat_vty_init(struct bsc_nat *nat)
 {
 	_nat = nat;
@@ -732,6 +925,7 @@ int bsc_nat_vty_init(struct bsc_nat *nat)
 	install_element_ve(&show_acc_lst_cmd);
 
 	install_element(ENABLE_NODE, &set_last_endp_cmd);
+	install_element(ENABLE_NODE, &block_new_conn_cmd);
 
 	/* nat group */
 	install_element(CONFIG_NODE, &cfg_nat_cmd);
@@ -748,6 +942,7 @@ int bsc_nat_vty_init(struct bsc_nat *nat)
 	install_element(NAT_NODE, &cfg_nat_bsc_ip_dscp_cmd);
 	install_element(NAT_NODE, &cfg_nat_bsc_ip_tos_cmd);
 	install_element(NAT_NODE, &cfg_nat_acc_lst_name_cmd);
+	install_element(NAT_NODE, &cfg_nat_no_acc_lst_name_cmd);
 	install_element(NAT_NODE, &cfg_nat_ussd_lst_name_cmd);
 	install_element(NAT_NODE, &cfg_nat_ussd_query_cmd);
 	install_element(NAT_NODE, &cfg_nat_ussd_token_cmd);
@@ -760,6 +955,15 @@ int bsc_nat_vty_init(struct bsc_nat *nat)
 
 	/* number rewriting */
 	install_element(NAT_NODE, &cfg_nat_number_rewrite_cmd);
+	install_element(NAT_NODE, &cfg_nat_smsc_addr_cmd);
+	install_element(NAT_NODE, &cfg_nat_smsc_tpdest_cmd);
+
+	install_element(NAT_NODE, &cfg_nat_pgroup_cmd);
+	install_element(NAT_NODE, &cfg_nat_no_pgroup_cmd);
+	install_node(&pgroup_node, config_write_pgroup);
+	install_default(PGROUP_NODE);
+	install_element(PGROUP_NODE, &cfg_pgroup_lac_cmd);
+	install_element(PGROUP_NODE, &cfg_pgroup_no_lac_cmd);
 
 	/* BSC subgroups */
 	install_element(NAT_NODE, &cfg_bsc_cmd);
@@ -773,7 +977,11 @@ int bsc_nat_vty_init(struct bsc_nat *nat)
 	install_element(NAT_BSC_NODE, &cfg_bsc_paging_cmd);
 	install_element(NAT_BSC_NODE, &cfg_bsc_desc_cmd);
 	install_element(NAT_BSC_NODE, &cfg_bsc_acc_lst_name_cmd);
+	install_element(NAT_BSC_NODE, &cfg_bsc_no_acc_lst_name_cmd);
 	install_element(NAT_BSC_NODE, &cfg_bsc_max_endps_cmd);
+	install_element(NAT_BSC_NODE, &cfg_bsc_old_grp_cmd);
+	install_element(NAT_BSC_NODE, &cfg_bsc_paging_grp_cmd);
+	install_element(NAT_BSC_NODE, &cfg_bsc_no_paging_grp_cmd);
 
 	mgcp_vty_init();
 
@@ -782,7 +990,8 @@ int bsc_nat_vty_init(struct bsc_nat *nat)
 
 
 /* called by the telnet interface... we have our own init above */
-int bsc_vty_init(void)
+int bsc_vty_init(const struct log_info *cat)
 {
+	logging_vty_add_cmds(cat);
 	return 0;
 }
diff --git a/src/osmo-bsc_nat/bsc_sccp.c b/src/osmo-bsc_nat/bsc_sccp.c
index 72de11201..de6b421ef 100644
--- a/src/osmo-bsc_nat/bsc_sccp.c
+++ b/src/osmo-bsc_nat/bsc_sccp.c
@@ -25,7 +25,7 @@
 
 #include <osmocom/sccp/sccp.h>
 
-#include <osmocore/talloc.h>
+#include <osmocom/core/talloc.h>
 
 #include <string.h>
 #include <time.h>
@@ -129,7 +129,7 @@ struct sccp_connections *create_sccp_src_ref(struct bsc_connection *bsc,
 	bsc_mgcp_init(conn);
 	llist_add_tail(&conn->list_entry, &bsc->nat->sccp_connections);
 	rate_ctr_inc(&bsc->cfg->stats.ctrg->ctr[BCFG_CTR_SCCP_CONN]);
-	counter_inc(bsc->cfg->nat->stats.sccp.conn);
+	osmo_counter_inc(bsc->cfg->nat->stats.sccp.conn);
 
 	LOGP(DNAT, LOGL_DEBUG, "Created 0x%x <-> 0x%x mapping for con %p\n",
 	     sccp_src_ref_to_int(&conn->real_ref),
diff --git a/src/osmo-bsc_nat/bsc_ussd.c b/src/osmo-bsc_nat/bsc_ussd.c
index c121abe5d..bbbeead9a 100644
--- a/src/osmo-bsc_nat/bsc_ussd.c
+++ b/src/osmo-bsc_nat/bsc_ussd.c
@@ -1,8 +1,8 @@
 /* USSD Filter Code */
 
 /*
- * (C) 2010 by Holger Hans Peter Freyther <zecke@selfish.org>
- * (C) 2010 by On-Waves
+ * (C) 2010-2011 by Holger Hans Peter Freyther <zecke@selfish.org>
+ * (C) 2010-2011 by On-Waves
  * All Rights Reserved
  *
  * This program is free software; you can redistribute it and/or modify
@@ -25,10 +25,10 @@
 #include <openbsc/ipaccess.h>
 #include <openbsc/socket.h>
 
-#include <osmocore/protocol/gsm_08_08.h>
-#include <osmocore/gsm0480.h>
-#include <osmocore/talloc.h>
-#include <osmocore/tlv.h>
+#include <osmocom/gsm/protocol/gsm_08_08.h>
+#include <osmocom/gsm/gsm0480.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/gsm/tlv.h>
 
 #include <osmocom/sccp/sccp.h>
 
@@ -36,14 +36,6 @@
 #include <string.h>
 #include <unistd.h>
 
-struct bsc_nat_ussd_con {
-	struct write_queue queue;
-	struct bsc_nat *nat;
-	int authorized;
-
-	struct timer_list auth_timeout;
-};
-
 static void ussd_auth_con(struct tlv_parsed *, struct bsc_nat_ussd_con *);
 
 static struct bsc_nat_ussd_con *bsc_nat_ussd_alloc(struct bsc_nat *nat)
@@ -66,9 +58,9 @@ static void bsc_nat_ussd_destroy(struct bsc_nat_ussd_con *con)
 	}
 
 	close(con->queue.bfd.fd);
-	bsc_unregister_fd(&con->queue.bfd);
-	bsc_del_timer(&con->auth_timeout);
-	write_queue_clear(&con->queue);
+	osmo_fd_unregister(&con->queue.bfd);
+	osmo_timer_del(&con->auth_timeout);
+	osmo_wqueue_clear(&con->queue);
 	talloc_free(con);
 }
 
@@ -103,7 +95,7 @@ static int forward_sccp(struct bsc_nat *nat, struct msgb *msg)
 	return 0;
 }
 
-static int ussd_read_cb(struct bsc_fd *bfd)
+static int ussd_read_cb(struct osmo_fd *bfd)
 {
 	int error;
 	struct bsc_nat_ussd_con *conn = bfd->data;
@@ -117,15 +109,21 @@ static int ussd_read_cb(struct bsc_fd *bfd)
 	}
 
 	LOGP(DNAT, LOGL_NOTICE, "MSG from USSD: %s proto: %d\n",
-		hexdump(msg->data, msg->len), msg->l2h[0]);
+		osmo_hexdump(msg->data, msg->len), msg->l2h[0]);
 	hh = (struct ipaccess_head *) msg->data;
 
 	if (hh->proto == IPAC_PROTO_IPACCESS) {
 		if (msg->l2h[0] == IPAC_MSGT_ID_RESP) {
 			struct tlv_parsed tvp;
-			ipaccess_idtag_parse(&tvp,
+			int ret;
+			ret = ipaccess_idtag_parse(&tvp,
 					     (unsigned char *) msg->l2h + 2,
 					     msgb_l2len(msg) - 2);
+			if (ret < 0) {
+				LOGP(DNAT, LOGL_ERROR, "ignoring IPA response "
+					"message with malformed TLVs\n");
+				return ret;
+			}
 			if (TLVP_PRESENT(&tvp, IPAC_IDTAG_UNITNAME))
 				ussd_auth_con(&tvp, conn);
 		}
@@ -170,7 +168,7 @@ static void ussd_auth_con(struct tlv_parsed *tvp, struct bsc_nat_ussd_con *conn)
 		bsc_nat_ussd_destroy(conn->nat->ussd_con);
 
 	LOGP(DNAT, LOGL_ERROR, "USSD token specified. USSD provider is connected.\n");
-	bsc_del_timer(&conn->auth_timeout);
+	osmo_timer_del(&conn->auth_timeout);
 	conn->authorized = 1;
 	conn->nat->ussd_con = conn;
 }
@@ -181,7 +179,7 @@ static void ussd_start_auth(struct bsc_nat_ussd_con *conn)
 
 	conn->auth_timeout.data = conn;
 	conn->auth_timeout.cb = ussd_auth_cb;
-	bsc_schedule_timer(&conn->auth_timeout, conn->nat->auth_timeout, 0);
+	osmo_timer_schedule(&conn->auth_timeout, conn->nat->auth_timeout, 0);
 
 	msg = msgb_alloc_headroom(4096, 128, "auth message");
 	if (!msg) {
@@ -193,7 +191,7 @@ static void ussd_start_auth(struct bsc_nat_ussd_con *conn)
 	bsc_do_write(&conn->queue, msg, IPAC_PROTO_IPACCESS);
 }
 
-static int ussd_listen_cb(struct bsc_fd *bfd, unsigned int what)
+static int ussd_listen_cb(struct osmo_fd *bfd, unsigned int what)
 {
 	struct bsc_nat_ussd_con *conn;
 	struct bsc_nat *nat;
@@ -211,7 +209,7 @@ static int ussd_listen_cb(struct bsc_fd *bfd, unsigned int what)
 	}
 
 	nat = (struct bsc_nat *) bfd->data;
-	counter_inc(nat->stats.ussd.reconn);
+	osmo_counter_inc(nat->stats.ussd.reconn);
 
 	conn = bsc_nat_ussd_alloc(nat);
 	if (!conn) {
@@ -220,14 +218,14 @@ static int ussd_listen_cb(struct bsc_fd *bfd, unsigned int what)
 		return -1;
 	}
 
-	write_queue_init(&conn->queue, 10);
+	osmo_wqueue_init(&conn->queue, 10);
 	conn->queue.bfd.data = conn;
 	conn->queue.bfd.fd = fd;
 	conn->queue.bfd.when = BSC_FD_READ;
 	conn->queue.read_cb = ussd_read_cb;
 	conn->queue.write_cb = bsc_write_cb;
 
-	if (bsc_register_fd(&conn->queue.bfd) < 0) {
+	if (osmo_fd_register(&conn->queue.bfd) < 0) {
 		LOGP(DNAT, LOGL_ERROR, "Failed to register USSD fd.\n");
 		bsc_nat_ussd_destroy(conn);
 		return -1;
@@ -251,7 +249,31 @@ int bsc_ussd_init(struct bsc_nat *nat)
 
 	nat->ussd_listen.data = nat;
 	return make_sock(&nat->ussd_listen, IPPROTO_TCP,
-			 ntohl(addr.s_addr), 5001, ussd_listen_cb);
+			 ntohl(addr.s_addr), 5001, 0, ussd_listen_cb, nat);
+}
+
+static int forward_ussd_simple(struct sccp_connections *con, struct msgb *input)
+{
+	struct msgb *copy;
+	struct bsc_nat_ussd_con *ussd;
+
+	if (!con->bsc->nat->ussd_con)
+		return -1;
+
+	copy = msgb_alloc_headroom(4096, 128, "forward bts");
+	if (!copy) {
+		LOGP(DNAT, LOGL_ERROR, "Allocation failed, not forwarding.\n");
+		return -1;
+	}
+
+	/* copy the data into the copy */
+	copy->l2h = msgb_put(copy, msgb_l2len(input));
+	memcpy(copy->l2h, input->l2h, msgb_l2len(input));
+
+	/* send it out */
+	ussd = con->bsc->nat->ussd_con;
+	bsc_do_write(&ussd->queue, copy, IPAC_PROTO_SCCP);
+	return 0;
 }
 
 static int forward_ussd(struct sccp_connections *con, const struct ussd_request *req,
@@ -303,6 +325,8 @@ int bsc_check_ussd(struct sccp_connections *con, struct bsc_nat_parsed *parsed,
 {
 	uint32_t len;
 	uint8_t msg_type;
+	uint8_t proto;
+	uint8_t ti;
 	struct gsm48_hdr *hdr48;
 	struct bsc_nat_acc_lst *lst;
 	struct ussd_request req;
@@ -318,6 +342,10 @@ int bsc_check_ussd(struct sccp_connections *con, struct bsc_nat_parsed *parsed,
 	if (!con->imsi)
 		return 0;
 
+	/* We have not verified the IMSI yet */
+	if (!con->authorized)
+		return 0;
+
 	if (!con->bsc->nat->ussd_lst_name)
 		return 0;
 	if (!con->bsc->nat->ussd_query)
@@ -333,31 +361,47 @@ int bsc_check_ussd(struct sccp_connections *con, struct bsc_nat_parsed *parsed,
 	if (!hdr48)
 		return 0;
 
+	proto = hdr48->proto_discr & 0x0f;
 	msg_type = hdr48->msg_type & 0xbf;
-	if (hdr48->proto_discr != GSM48_PDISC_NC_SS || msg_type != GSM0480_MTYPE_REGISTER)
-		return 0;
-
-	/* now check if it is a IMSI we care about */
-	lst = bsc_nat_acc_lst_find(con->bsc->nat, con->bsc->nat->ussd_lst_name);
-	if (!lst)
+	ti = (hdr48->proto_discr & 0x70) >> 4;
+	if (proto != GSM48_PDISC_NC_SS)
 		return 0;
 
-	if (bsc_nat_lst_check_allow(lst, con->imsi) != 0)
-		return 0;
-
-	/* now decode the message and see if we really want to handle it */
-	memset(&req, 0, sizeof(req));
-	if (gsm0480_decode_ussd_request(hdr48, len, &req) != 1)
-		return 0;
-	if (req.text[0] == 0xff)
-		return 0;
-
-	if (strcmp(req.text, con->bsc->nat->ussd_query) != 0)
-		return 0;
+	if (msg_type == GSM0480_MTYPE_REGISTER) {
+
+		/* now check if it is a IMSI we care about */
+		lst = bsc_nat_acc_lst_find(con->bsc->nat,
+					   con->bsc->nat->ussd_lst_name);
+		if (!lst)
+			return 0;
+
+		if (bsc_nat_lst_check_allow(lst, con->imsi) != 0)
+			return 0;
+
+		/* now decode the message and see if we really want to handle it */
+		memset(&req, 0, sizeof(req));
+		if (gsm0480_decode_ussd_request(hdr48, len, &req) != 1)
+			return 0;
+		if (req.text[0] == 0xff)
+			return 0;
+
+		if (regexec(&con->bsc->nat->ussd_query_re,
+			    req.text, 0, NULL, 0) == REG_NOMATCH)
+			return 0;
+
+		/* found a USSD query for our subscriber */
+		LOGP(DNAT, LOGL_NOTICE, "Found USSD query for %s\n", con->imsi);
+		con->ussd_ti[ti] = 1;
+		if (forward_ussd(con, &req, msg) != 0)
+			return 0;
+		return 1;
+	} else if (msg_type == GSM0480_MTYPE_FACILITY && con->ussd_ti[ti]) {
+		LOGP(DNAT, LOGL_NOTICE, "Forwarding message part of TI: %d %s\n",
+		     ti, con->imsi);
+		if (forward_ussd_simple(con, msg) != 0)
+			return 0;
+		return 1;
+	}
 
-	/* found a USSD query for our subscriber */
-	LOGP(DNAT, LOGL_NOTICE, "Found USSD query for %s\n", con->imsi);
-	if (forward_ussd(con, &req, msg) != 0)
-		return 0;
-	return 1;
+	return 0;
 }