USSD MAP external interface over SUP

15 files changed, 1116 insertions, 40 deletions

diff --git a/openbsc/include/openbsc/gprs_gsup_messages.h b/openbsc/include/openbsc/gprs_gsup_messages.h
index fb83b25ee..d812204e0 100644
--- a/openbsc/include/openbsc/gprs_gsup_messages.h
+++ b/openbsc/include/openbsc/gprs_gsup_messages.h
@@ -52,6 +52,8 @@ enum gprs_gsup_iei {
 };
 
 enum gprs_gsup_message_type {
+    GPRS_GSUP_MSGT_MAP                      = 0b01111111,
+
 	GPRS_GSUP_MSGT_UPDATE_LOCATION_REQUEST	= 0b00000100,
 	GPRS_GSUP_MSGT_UPDATE_LOCATION_ERROR	= 0b00000101,
 	GPRS_GSUP_MSGT_UPDATE_LOCATION_RESULT	= 0b00000110,
diff --git a/openbsc/include/openbsc/gsm_04_80.h b/openbsc/include/openbsc/gsm_04_80.h
index 0a6065234..bf874cbb9 100644
--- a/openbsc/include/openbsc/gsm_04_80.h
+++ b/openbsc/include/openbsc/gsm_04_80.h
@@ -8,8 +8,8 @@
 struct gsm_subscriber_connection;
 
 int gsm0480_send_ussd_response(struct gsm_subscriber_connection *conn,
-			       const struct msgb *in_msg, const char* response_text, 
-			       const struct ussd_request *req);
+                   const struct msgb *in_msg, const char* response_text,
+                   const struct ussd_request *req, uint8_t code, uint8_t ctype, uint8_t mtype);
 int gsm0480_send_ussd_reject(struct gsm_subscriber_connection *conn,
 			     const struct msgb *msg, 
 			     const struct ussd_request *request);
diff --git a/openbsc/include/openbsc/gsm_data.h b/openbsc/include/openbsc/gsm_data.h
index 9a15653b2..2b79148fe 100644
--- a/openbsc/include/openbsc/gsm_data.h
+++ b/openbsc/include/openbsc/gsm_data.h
@@ -288,7 +288,7 @@ struct gsm_network {
 	uint64_t exten_prefix;
 
 	/* control interface */
-	struct ctrl_handle *ctrl;
+    struct ctrl_handle *ctrl;
 };
 
 struct osmo_esme;
diff --git a/openbsc/include/openbsc/gsm_sup.h b/openbsc/include/openbsc/gsm_sup.h
index d14b036b2..fbfb2f9df 100644
--- a/openbsc/include/openbsc/gsm_sup.h
+++ b/openbsc/include/openbsc/gsm_sup.h
@@ -3,6 +3,7 @@
 
 #include <openbsc/debug.h>
 #include <openbsc/gsm_subscriber.h>
+#include <osmocom/gsm/gsm0480.h>
 
 #define LOGGSUBSCRP(level, subscr, fmt, args...) \
 	LOGP(DSUP, level, "SUBSCR(%s) " fmt, \
@@ -13,4 +14,8 @@ int subscr_query_auth_info(struct gsm_subscriber *subscr);
 int subscr_location_update(struct gsm_subscriber *subscr);
 int sup_init(struct gsm_network *net);
 
+
+int subscr_tx_uss_message(struct ss_request *req,
+			  struct gsm_subscriber *subscr);
+
 #endif /* _GSM_SUP_H */
diff --git a/openbsc/include/openbsc/sip.h b/openbsc/include/openbsc/sip.h
index 8a7a56264..581f3f3c5 100644
--- a/openbsc/include/openbsc/sip.h
+++ b/openbsc/include/openbsc/sip.h
@@ -4,6 +4,10 @@
 #include <openbsc/sip_client.h>
 #include <openbsc/reg_proxy.h>
 #include <osip2/osip.h>
+#include <osmocom/gsm/gsm0480.h>
+
+int tx_ss_handle(struct sip_client *sip_client, osip_t *osip, struct ss_request *ss,
+		 const char *extention);
 
 int tx_sip_register(struct sip_client *sip_client, osip_t *osip, char *imsi);
 
diff --git a/openbsc/include/openbsc/ussd.h b/openbsc/include/openbsc/ussd.h
index 266546811..a84f51fbd 100644
--- a/openbsc/include/openbsc/ussd.h
+++ b/openbsc/include/openbsc/ussd.h
@@ -7,4 +7,7 @@
 
 int handle_rcv_ussd(struct gsm_subscriber_connection *conn, struct msgb *msg);
 
+
+int on_ussd_response(const struct ss_request* req, const char* extention);
+
 #endif
diff --git a/openbsc/src/libmsc/gsm_04_80.c b/openbsc/src/libmsc/gsm_04_80.c
index b30f9ee5f..7b21c2615 100644
--- a/openbsc/src/libmsc/gsm_04_80.c
+++ b/openbsc/src/libmsc/gsm_04_80.c
@@ -63,13 +63,17 @@ static inline unsigned char *msgb_push_TLV1(struct msgb *msgb, uint8_t tag,
 /* Send response to a mobile-originated ProcessUnstructuredSS-Request */
 int gsm0480_send_ussd_response(struct gsm_subscriber_connection *conn,
 			       const struct msgb *in_msg, const char *response_text,
-			       const struct ussd_request *req)
+			       const struct ussd_request *req,
+			       uint8_t code,
+			       uint8_t ctype,
+			       uint8_t mtype)
 {
 	struct msgb *msg = gsm48_msgb_alloc();
 	struct gsm48_hdr *gh;
 	uint8_t *ptr8;
 	int response_len;
 
+#if 1
 	/* First put the payload text into the message */
 	ptr8 = msgb_put(msg, 0);
 	gsm_7bit_encode_n_ussd(ptr8, msgb_tailroom(msg), response_text, &response_len);
@@ -80,31 +84,65 @@ int gsm0480_send_ussd_response(struct gsm_subscriber_connection *conn,
 
 	/* Pre-pend the DCS octet string */
 	msgb_push_TLV1(msg, ASN1_OCTET_STRING_TAG, 0x0F);
+#else
+	response_len = strlen(response_text);
+	if (response_len > MAX_LEN_USSD_STRING)
+		response_len = MAX_LEN_USSD_STRING;
+
+	ptr8 = msgb_put(msg, 0);
+	memcpy(ptr8, response_text, response_len);
+	msgb_put(msg, response_len);
+
+	/* Then wrap it as an Octet String */
+	msgb_wrap_with_TL(msg, ASN1_OCTET_STRING_TAG);
+
+	/* Pre-pend the DCS octet string */
+	msgb_push_TLV1(msg, ASN1_OCTET_STRING_TAG, 0xf4);
+#endif
+
 
 	/* Then wrap these as a Sequence */
 	msgb_wrap_with_TL(msg, GSM_0480_SEQUENCE_TAG);
 
-	/* Pre-pend the operation code */
-	msgb_push_TLV1(msg, GSM0480_OPERATION_CODE,
-			GSM0480_OP_CODE_PROCESS_USS_REQ);
+	if (ctype == GSM0480_CTYPE_RETURN_RESULT) {
+		/* Pre-pend the operation code */
+		msgb_push_TLV1(msg, GSM0480_OPERATION_CODE, code);
 
-	/* Wrap the operation code and IA5 string as a sequence */
-	msgb_wrap_with_TL(msg, GSM_0480_SEQUENCE_TAG);
+		/* Wrap the operation code and IA5 string as a sequence */
+		msgb_wrap_with_TL(msg, GSM_0480_SEQUENCE_TAG);
 
-	/* Pre-pend the invoke ID */
-	msgb_push_TLV1(msg, GSM0480_COMPIDTAG_INVOKE_ID, req->invoke_id);
+		/* Pre-pend the invoke ID */
+		msgb_push_TLV1(msg, GSM0480_COMPIDTAG_INVOKE_ID, req->invoke_id);
+	} else if (ctype == GSM0480_CTYPE_INVOKE) {
+		/* Pre-pend the operation code */
+		msgb_push_TLV1(msg, GSM0480_OPERATION_CODE, code);
 
-	/* Wrap this up as a Return Result component */
-	msgb_wrap_with_TL(msg, GSM0480_CTYPE_RETURN_RESULT);
+		/* Pre-pend the invoke ID */
+		msgb_push_TLV1(msg, GSM0480_COMPIDTAG_INVOKE_ID, req->invoke_id);
+	} else {
+		abort();
+	}
 
-	/* Wrap the component in a Facility message */
-	msgb_wrap_with_TL(msg, GSM0480_IE_FACILITY);
+	/* Wrap this up as a Return Result component */
+	msgb_wrap_with_TL(msg, ctype);
+
+	if (mtype == GSM0480_MTYPE_REGISTER ||
+		mtype == GSM0480_MTYPE_RELEASE_COMPLETE) {
+		/* Wrap the component in a Facility message */
+		msgb_wrap_with_TL(msg, GSM0480_IE_FACILITY);
+	} else if (mtype == GSM0480_MTYPE_FACILITY) {
+		uint8_t *data = msgb_push(msg, 1);
+		data[0] = msg->len - 1;
+	} else {
+		abort();
+	}
 
 	/* And finally pre-pend the L3 header */
 	gh = (struct gsm48_hdr *) msgb_push(msg, sizeof(*gh));
 	gh->proto_discr = GSM48_PDISC_NC_SS | req->transaction_id
-					| (1<<7);  /* TI direction = 1 */
-	gh->msg_type = GSM0480_MTYPE_RELEASE_COMPLETE;
+			| (1<<7);  /* TI direction = 1 */
+
+	gh->msg_type = mtype;
 
 	return gsm0808_submit_dtap(conn, msg, 0, 0);
 }
diff --git a/openbsc/src/libmsc/gsm_sup.c b/openbsc/src/libmsc/gsm_sup.c
index ab1766420..b586fa3f3 100644
--- a/openbsc/src/libmsc/gsm_sup.c
+++ b/openbsc/src/libmsc/gsm_sup.c
@@ -28,6 +28,150 @@
 #include <openbsc/gsm_04_08_gprs.h>
 #include <openbsc/gprs_gsup_messages.h>
 #include <openbsc/gprs_gsup_client.h>
+#include <openbsc/osmo_msc.h>
+#include <openbsc/gprs_utils.h>
+#include <openbsc/ussd.h>
+
+enum {
+    FMAP_MSISDN        = 0x80
+};
+
+static int subscr_uss_message(struct msgb *msg,
+			      struct ss_request *req,
+			      const char* extention)
+{
+	size_t bcd_len = 0;
+	uint8_t *gsup_indicator;
+
+	gsup_indicator = msgb_put(msg, 4);
+
+	/* First byte should always be GPRS_GSUP_MSGT_MAP */
+	gsup_indicator[0] = GPRS_GSUP_MSGT_MAP;
+	gsup_indicator[1] = req->message_type;
+	/* TODO ADD tid */
+	gsup_indicator[2] = req->component_type;
+
+	/* invokeId */
+	msgb_tlv_put(msg, GSM0480_COMPIDTAG_INVOKE_ID, 1, &req->invoke_id);
+
+	/* opCode */
+	msgb_tlv_put(msg, GSM0480_OPERATION_CODE, 1, &req->opcode);
+
+	if (req->ussd_text_len > 0) {
+		//msgb_tlv_put(msg, ASN1_OCTET_STRING_TAG, 1, &req->ussd_text_language);
+		msgb_tlv_put(msg, ASN1_OCTET_STRING_TAG, req->ussd_text_len, req->ussd_text);
+	}
+
+	if (extention) {
+		uint8_t bcd_buf[32];
+		bcd_len = gsm48_encode_bcd_number(bcd_buf, sizeof(bcd_buf), 0,
+						  extention);
+		msgb_tlv_put(msg, FMAP_MSISDN, bcd_len - 1, &bcd_buf[1]);
+	}
+
+	/* fill actual length */
+	gsup_indicator[3] = 3 + 3 + (req->ussd_text_len + 2) + (bcd_len + 2);;
+
+	/* wrap with GSM0480_CTYPE_INVOKE */
+	// gsm0480_wrap_invoke(msg, req->opcode, invoke_id);
+	// gsup_indicator = msgb_push(msgb, 1);
+	// gsup_indicator[0] = GPRS_GSUP_MSGT_MAP;
+	return 0;
+}
+
+
+int subscr_tx_uss_message(struct ss_request *req,
+			  struct gsm_subscriber *subscr)
+{
+    struct msgb *msg = gprs_gsup_msgb_alloc();
+    if (!msg)
+        return -ENOMEM;
+
+    //GSM0480_OP_CODE_PROCESS_USS_REQ
+    subscr_uss_message(msg, req, subscr->extension);
+
+    return gprs_gsup_client_send(subscr->group->net->sup_client, msg);
+}
+
+
+static int rx_uss_message_parse(struct ss_request *ss,
+				const uint8_t* data,
+				size_t len,
+				char* extention,
+				size_t extention_len)
+{
+	const uint8_t* const_data = data;
+
+	if (len < 1 + 2 + 3 + 3)
+		return -1;
+
+	/* skip GPRS_GSUP_MSGT_MAP */
+	ss->message_type = *(++const_data);
+	ss->component_type = *(++const_data);
+	const_data += 2;
+
+	//
+	if (*const_data != GSM0480_COMPIDTAG_INVOKE_ID) {
+		return -1;
+	}
+	const_data += 2;
+	ss->invoke_id = *const_data;
+	const_data++;
+
+	//
+	if (*const_data != GSM0480_OPERATION_CODE) {
+		return -1;
+	}
+	const_data += 2;
+	ss->opcode = *const_data;
+	const_data++;
+
+
+	while (const_data - data < len) {
+		uint8_t len;
+		switch (*const_data) {
+		case ASN1_OCTET_STRING_TAG:
+			len = *(++const_data);
+			strncpy((char*)ss->ussd_text,
+				(const char*)++const_data,
+				(len > MAX_LEN_USSD_STRING) ? MAX_LEN_USSD_STRING : len);
+			const_data += len;
+			break;
+
+		case FMAP_MSISDN:
+			len = *(++const_data);
+			gsm48_decode_bcd_number(extention,
+						extention_len,
+						const_data,
+						0);
+			const_data += len + 1;
+			break;
+		default:
+			DEBUGP(DMM, "Unknown code: %d\n", *const_data);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+int rx_uss_message(const uint8_t* data, size_t len)
+{
+	char extention[32] = {0};
+	struct ss_request ss;
+	memset(&ss, 0, sizeof(ss));
+
+	if (rx_uss_message_parse(&ss, data, len, extention, sizeof(extention))) {
+		LOGP(DSUP, LOGL_ERROR, "Can't parse uss message\n");
+		return -1;
+	}
+
+	LOGP(DSUP, LOGL_ERROR, "Got invoke_id=0x%02x opcode=0x%02x facility=0x%02x text=%s\n",
+	     ss.invoke_id, ss.opcode, ss.component_type, ss.ussd_text);
+
+	return on_ussd_response(&ss, extention);
+}
+
 
 static int subscr_tx_sup_message(struct gprs_gsup_client *sup_client,
 								 struct gsm_subscriber *subscr,
@@ -289,6 +433,10 @@ int subscr_rx_sup_message(struct gprs_gsup_client *sup_client, struct msgb *msg)
 	struct gprs_gsup_message gsup_msg = {0};
 	struct gsm_subscriber *subscr;
 
+    if (*data == GPRS_GSUP_MSGT_MAP) {
+        return rx_uss_message(data, data_len);
+    }
+
 	rc = gprs_gsup_decode(data, data_len, &gsup_msg);
 	if (rc < 0) {
 		LOGP(DSUP, LOGL_ERROR,
diff --git a/openbsc/src/libmsc/ussd.c b/openbsc/src/libmsc/ussd.c
index 7f01eae71..c81f777c7 100644
--- a/openbsc/src/libmsc/ussd.c
+++ b/openbsc/src/libmsc/ussd.c
@@ -33,18 +33,233 @@
 #include <openbsc/gsm_subscriber.h>
 #include <openbsc/debug.h>
 #include <openbsc/osmo_msc.h>
+#include <openbsc/gsm_sup.h>
+#include <openbsc/ussd.h>
+#include <osmocom/gsm/gsm0480.h>
 
-/* Declarations of USSD strings to be recognised */
-const char USSD_TEXT_OWN_NUMBER[] = "*#100#";
+struct gsm_ussd {
+	struct llist_head ussqueue;
 
-/* Forward declarations of network-specific handler functions */
-static int send_own_number(struct gsm_subscriber_connection *conn, const struct msgb *msg, const struct ussd_request *req);
+	uint8_t uniq_id;   /**<  System wide uniq ID */
+
+	uint8_t invoke_id;
+	uint8_t transaction_id;
+
+	uint8_t current_transaction_id;
+
+	struct gsm_subscriber_connection *conn;
+};
+
+static uint64_t s_uniq_ussd_sessiod_id = 0;
+static LLIST_HEAD(s_active_ussd_sessions);
+
+static struct llist_head *get_active_ussd_sessions(void)
+{
+	return &s_active_ussd_sessions;
+}
+
+
+static struct gsm_ussd* ussd_session_alloc(struct gsm_subscriber_connection* conn)
+{
+	struct gsm_network* net = conn->bts->network;
+	struct gsm_ussd* m = talloc_zero(net, struct gsm_ussd);
+	if (!m)
+		return NULL;
+
+	m->conn = conn;
+	m->uniq_id = s_uniq_ussd_sessiod_id++;
+
+	INIT_LLIST_HEAD(&m->ussqueue);
+	llist_add_tail(&m->ussqueue, &s_active_ussd_sessions);
+
+	DEBUGP(DMM, "Alloc USSD session: %d\n", m->uniq_id);
+	return m;
+}
+
+static void ussd_session_free(struct gsm_ussd* s)
+{
+	DEBUGP(DMM, "Free USSD session: %d\n", s->uniq_id);
+	llist_del(&s->ussqueue);
+	talloc_free(s);
+}
+
+static struct gsm_ussd* get_by_uniq_id(uint8_t invoke_id)
+{
+	struct gsm_ussd* c;
+	llist_for_each_entry(c, get_active_ussd_sessions(), ussqueue) {
+		if (c->uniq_id == invoke_id) {
+			DEBUGP(DMM, "uniq invoke_id %d has %s extention\n",
+			       invoke_id, c->conn->subscr->extension);
+			return c;
+		}
+	}
+
+	DEBUGP(DMM, "uniq invoke_id %d hasn't been found\n", invoke_id);
+	return NULL;
+}
+
+static struct gsm_ussd* get_by_id(struct gsm_subscriber_connection *conn, uint8_t invoke_id)
+{
+	struct gsm_ussd* c;
+	llist_for_each_entry(c, get_active_ussd_sessions(), ussqueue) {
+		if (c->conn == conn && c->invoke_id == invoke_id) {
+			DEBUGP(DMM, "invoke_id %d has %s extention\n",
+			       invoke_id, c->conn->subscr->extension);
+			return c;
+		}
+	}
+
+	DEBUGP(DMM, "invoke_id %d hasn't been found\n", invoke_id);
+	return NULL;
+}
+
+int on_ussd_response(const struct ss_request *req, const char *extention)
+{
+	struct ussd_request ussd_req;
+	struct gsm_ussd* ussdq = get_by_uniq_id(req->invoke_id);
+	memset(&ussd_req, 0, sizeof(ussd_req));
+	int rc;
+	uint8_t mtype;
+
+	switch (req->opcode) {
+	case GSM0480_OP_CODE_USS_NOTIFY:
+		DEBUGP(DMM, "Network originated USSD Notify is not supported yet!\n");
+
+		if (!ussdq) {
+			mtype = GSM0480_MTYPE_REGISTER;
+		} else {
+			mtype = GSM0480_MTYPE_FACILITY;
+		}
+
+		return -ENOTSUP;
+	case GSM0480_OP_CODE_PROCESS_USS_REQ:
+		if (!ussdq) {
+			DEBUGP(DMM, "Network originated Process USSD Request is not supported yet!\n");
+			// TODO SUP Reject
+			return -ENOTSUP;
+		}
+
+		mtype = GSM0480_MTYPE_RELEASE_COMPLETE;
+		ussd_req.transaction_id = ussdq->transaction_id;
+		break;
+	case GSM0480_OP_CODE_USS_REQUEST:
+		if (!ussdq) {
+			DEBUGP(DMM, "No session was found for invoke_id: %d\n", req->invoke_id);
+			return -EINVAL;
+		}
+
+		mtype = GSM0480_MTYPE_FACILITY;
+		ussd_req.transaction_id = ussdq->current_transaction_id;
+		break;
+	default:
+		// TODO SUP Reject
+		return -EINVAL;
+	}
+
+	ussd_req.invoke_id = ussdq->invoke_id;
+
+	if (req->ussd_text[0]) {
+		rc = gsm0480_send_ussd_response(ussdq->conn,
+						NULL,
+						(const char *)req->ussd_text,
+						&ussd_req,
+						req->opcode,
+						req->component_type,
+						mtype);
+	} else {
+		rc = gsm0480_send_ussd_reject(ussdq->conn, NULL, &ussd_req);
+	}
+	if (rc || mtype == GSM0480_MTYPE_RELEASE_COMPLETE) {
+		ussd_session_free(ussdq);
+		msc_release_connection(ussdq->conn);
+	}
 
+	return rc;
+}
 
 /* Entrypoint - handler function common to all mobile-originated USSDs */
 int handle_rcv_ussd(struct gsm_subscriber_connection *conn, struct msgb *msg)
 {
 	int rc;
+	struct gsm48_hdr *gh;
+	struct ss_request req;
+	struct gsm_ussd* ussdq = NULL;
+	struct ussd_request ussd_req;
+
+	memset(&req, 0, sizeof(req));
+	memset(&ussd_req, 0, sizeof(ussd_req));
+
+	DEBUGP(DMM, "handle ussd: %s\n", msgb_hexdump(msg));
+
+	gh = msgb_l3(msg);
+	rc = gsm0480_decode_ss_request(gh, msgb_l3len(msg), &req);
+
+	//if (req.ussd_text[0] == 0xFF)
+	//	req.ussd_text[0] = '\0';
+
+	if (!rc) {
+		DEBUGP(DMM, "Unhandled SS\n");
+		goto failed;
+	}
+
+	ussdq = get_by_id(conn, req.invoke_id);
+
+	// TODO FIXME !!!! Replace by message_type
+	switch (req.opcode) {
+	case GSM0480_OP_CODE_PROCESS_USS_REQ:
+		if (ussdq) {
+			/* new session with the same id as an open session, destroy both */
+			DEBUGP(DMM, "Duplicate session? invoke_id: %d\n", req.invoke_id);
+			goto failed;
+		}
+
+		if (req.component_type != GSM0480_CTYPE_INVOKE) {
+			DEBUGP(DMM, "processUSS with component_type 0x%02x\n", req.component_type);
+			goto failed;
+		}
+
+		ussdq = ussd_session_alloc(conn);
+		if (!ussdq) {
+			DEBUGP(DMM, "Failed to create new session\n");
+			goto failed;
+		}
+
+		ussdq->conn = conn;
+		ussdq->invoke_id = req.invoke_id;
+		ussdq->transaction_id = req.transaction_id;
+		break;
+
+	case GSM0480_OP_CODE_USS_REQUEST:
+		if (!ussdq) {
+			DEBUGP(DMM, "no session found for USS_REQUEST with invoke_id=%d\n", req.invoke_id);
+			goto failed;
+		}
+		if (req.component_type != GSM0480_CTYPE_RETURN_RESULT) {
+			DEBUGP(DMM, "USS with component_type 0x%02x\n", req.component_type);
+			goto failed;
+		}
+
+		ussdq->current_transaction_id = req.transaction_id;
+		break;
+
+	default:
+		DEBUGP(DMM, "Unhandled opcode: 0x%02x, component_type: 0x%02x, text: %s\n",
+		       req.opcode, req.component_type, req.ussd_text);
+		goto failed;
+	}
+
+	// ACHTUNG! FIXME!! FIXME!! Introduce transaction ID instead
+	// Override Invoke ID
+	req.invoke_id = ussdq->uniq_id;
+	rc = subscr_tx_uss_message(&req, conn->subscr);
+	if (rc) {
+		DEBUGP(DMM, "Unable tp send uss over sup reason: %d\n", rc);
+		goto failed;
+	}
+
+	return 0;
+
+#if 0
 	struct ussd_request req;
 	struct gsm48_hdr *gh;
 
@@ -66,15 +281,36 @@ int handle_rcv_ussd(struct gsm_subscriber_connection *conn, struct msgb *msg)
 		DEBUGP(DMM, "USSD: Own number requested\n");
 		rc = send_own_number(conn, msg, &req);
 	} else {
-		DEBUGP(DMM, "Unhandled USSD %s\n", req.text);
-		rc = gsm0480_send_ussd_reject(conn, msg, &req);
+		rc = subscr_tx_uss_message(req, conn->subscr);
+
+
+		//TODO:
 	}
+#endif
 
+failed:
+	// TODO handle error on SUP end
+	if (ussdq) {
+		ussd_session_free(ussdq);
+	}
+
+	ussd_req.invoke_id = req.invoke_id;
+	ussd_req.transaction_id = req.transaction_id;
+	gsm0480_send_ussd_reject(conn, msg, &ussd_req);
 	/* check if we can release it */
 	msc_release_connection(conn);
 	return rc;
 }
 
+#if 0
+
+/* Declarations of USSD strings to be recognised */
+const char USSD_TEXT_OWN_NUMBER[] = "*#100#";
+
+/* Forward declarations of network-specific handler functions */
+static int send_own_number(struct gsm_subscriber_connection *conn, const struct msgb *msg, const struct ussd_request *req);
+
+
 /* A network-specific handler function */
 static int send_own_number(struct gsm_subscriber_connection *conn, const struct msgb *msg, const struct ussd_request *req)
 {
@@ -85,3 +321,4 @@ static int send_own_number(struct gsm_subscriber_connection *conn, const struct
 	snprintf(response_string, sizeof(response_string), "Your extension is %s\r", own_number);
 	return gsm0480_send_ussd_response(conn, msg, response_string, req);
 }
+#endif
diff --git a/openbsc/src/osmo-nitb/bsc_hack.c b/openbsc/src/osmo-nitb/bsc_hack.c
index c3fc8c614..32db75d53 100644
--- a/openbsc/src/osmo-nitb/bsc_hack.c
+++ b/openbsc/src/osmo-nitb/bsc_hack.c
@@ -324,13 +324,13 @@ int main(int argc, char **argv)
 	}
 	printf("DB: Database prepared.\n");
 
-	if (bsc_gsmnet->auth_policy == GSM_AUTH_POLICY_REMOTE) {
+    //if (bsc_gsmnet->auth_policy == GSM_AUTH_POLICY_REMOTE) {
 		rc = sup_init(bsc_gsmnet);
 		if (rc < 0) {
 			LOGP(DSUP, LOGL_FATAL, "Cannot set up subscriber management\n");
 			exit(2);
 		}
-	}
+    //}
 
 	/* setup the timer */
 	db_sync_timer.cb = db_sync_timer_cb;
diff --git a/openbsc/src/reg-proxy/reg_proxy.c b/openbsc/src/reg-proxy/reg_proxy.c
index 3ad9f0c71..9444df7e1 100644
--- a/openbsc/src/reg-proxy/reg_proxy.c
+++ b/openbsc/src/reg-proxy/reg_proxy.c
@@ -52,7 +52,7 @@
 
 static const char *sip_src_ip = "127.0.0.1";
 static const char *sip_dst_ip = "127.0.0.1";
-static u_int16_t src_port = 5050;
+static u_int16_t src_port = 5150;
 static u_int16_t dst_port = 5060;
 
 struct log_info_cat ipa_proxy_test_cat[] = {
@@ -294,9 +294,19 @@ int main(int argc, char **argv)
 
 	while (1) {
 		log_reset_context();
-		osmo_select_main(0);
+		osmo_select_main(0); //<-- TIMER handling
 		osip_nict_execute(reg->osip);
 		osip_timers_nict_execute(reg->osip);
 
+		osip_ict_execute(reg->osip);
+		osip_timers_ict_execute(reg->osip);
+
+		osip_nist_execute(reg->osip);
+		osip_timers_nist_execute(reg->osip);
+
+		osip_ist_execute(reg->osip);
+		osip_timers_ist_execute(reg->osip);
+
+		osip_retransmissions_execute(reg->osip);
 	}
 }
diff --git a/openbsc/src/reg-proxy/sip.c b/openbsc/src/reg-proxy/sip.c
index 6b52aa5da..e09f452f0 100644
--- a/openbsc/src/reg-proxy/sip.c
+++ b/openbsc/src/reg-proxy/sip.c
@@ -1,8 +1,12 @@
 #include <openbsc/sip_client.h>
 #include <openbsc/reg_proxy.h>
 #include <openbsc/sup.h>
+#include <osmocom/gsm/gsm0480.h>
 #include <osip2/osip.h>
-
+#include <osip2/osip_dialog.h>
+#include <osip2/osip_fifo.h>
+#include <osip2/osip_time.h>
+#include <assert.h>
 
 #define MESSAGE_MAX_LENGTH 4000
 #define MAX_ADDR_STR 128
@@ -14,6 +18,17 @@
 
 
 
+int get_seqnum(void)
+{
+	static int seq_num = 1;
+	if (seq_num < (1<<30)) {
+		seq_num++;
+	}
+	else {
+		seq_num = 1;
+	}
+	return seq_num;
+}
 
 int sip_send(struct sip_client *sip_client, osip_t *osip,
              osip_message_t *msg, osip_fsm_type_t transaction_type)
@@ -29,6 +44,7 @@ int sip_send(struct sip_client *sip_client, osip_t *osip,
 	}
 
 	osip_transaction_set_your_instance(transaction, sip_client);
+	osip_transaction_set_reserved6(transaction, osip);
 
 	sip_event = osip_new_outgoing_sipmessage(msg);
 	if (!sip_event) {
@@ -65,7 +81,6 @@ int tx_sip_register(struct sip_client *sip_client, osip_t *osip, char *imsi)
 {
 	osip_message_t *reg_msg;
 
-	static int seq_num = 1;
 	char *call_id_num = NULL;
 	char *seq_num_str = NULL;
 	osip_call_id_t *call_id;
@@ -114,15 +129,8 @@ int tx_sip_register(struct sip_client *sip_client, osip_t *osip, char *imsi)
 		return -1;
 	}
 
-	if (seq_num < (1<<30)) {
-		seq_num++;
-	}
-	else {
-		seq_num = 1;
-	}
-
 	seq_num_str = (char *)osip_malloc(11);
-	sprintf(seq_num_str,"%i", seq_num);
+	sprintf(seq_num_str,"%i", get_seqnum());
 	osip_cseq_set_number(cseq, seq_num_str);
 	osip_cseq_set_method(cseq, osip_strdup("REGISTER"));
 	reg_msg->cseq = cseq;
@@ -200,12 +208,12 @@ int sip_cb_send(osip_transaction_t *tr, osip_message_t *sip_msg, char *host, int
 	size_t msg_len;
 	char *msg_p;
 	struct msgb *msg = sip_msgb_alloc();
-
+	int err;
 	struct sip_client *sip_client = osip_transaction_get_your_instance(tr);
 	printf("SIP Send Msg\n");
 	
-	if(osip_message_to_str(sip_msg, &msg_p, &msg_len) != 0){
-		printf("SIP failed to convert message\n");
+	if ((err = osip_message_to_str(sip_msg, &msg_p, &msg_len)) != 0){
+		printf("SIP failed to convert message: %d\n", err);
 		return -1;
 	}
 	printf("SIP convert message ok\n");
@@ -215,6 +223,15 @@ int sip_cb_send(osip_transaction_t *tr, osip_message_t *sip_msg, char *host, int
 		return -1;
 	}
 
+	if (sip_client == NULL) {
+		osip_dialog_t* diag = (osip_dialog_t* )sip_msg->application_data;
+		if (diag == NULL) {
+			printf("Unable to send:\n%s\n", msg_p);
+			return -1;
+		}
+		sip_client = (struct sip_client *)diag->your_instance;
+	}
+
 	printf("SIP msg_p != NULL OK!, msg_len = %d\n", msg_len);
 	memcpy(msg->data, (uint8_t*)msg_p, msg_len);
 	msg->data_len = msg_len;
@@ -277,6 +294,174 @@ void sip_cb_rcv6xx(int type, osip_transaction_t *pott,osip_message_t *pomt)
 }
 
 
+
+void sip_cb_ict_rcv2xx(int type, osip_transaction_t *tr, osip_message_t *sip_msg)
+{
+	printf("OSIP_ICT_STATUS_2XX_RECEIVED\n");
+	//osip_contact_t *contact;
+	osip_to_t* to;
+	osip_from_t* from;
+	struct sip_client *sip_client = osip_transaction_get_your_instance(tr);
+	osip_t* osip = osip_transaction_get_reserved6(tr);
+
+	//struct reg_proxy *reg = sip_client->data;
+
+	//char request[32];
+	char from_str[32];
+	char to_str[32];
+	int rc;
+
+	//osip_message_get_contact(sip_msg, 0, &contact);
+	//strncpy(request, contact->url->username, sizeof(request));
+
+	from = osip_message_get_from(sip_msg);
+	strncpy(from_str, from->url->username, sizeof(from_str));
+
+	to = osip_message_get_to(sip_msg);
+	strncpy(to_str, to->url->username, sizeof(to_str));
+
+	printf("FROM: %s TO: %s\n", from_str, to_str);
+	osip_dialog_t *dialog;
+	if (MSG_IS_RESPONSE_FOR(sip_msg, "INVITE")) {
+		//dialog = my_application_search_existing_dialog(sip_msg);
+		//if (dialog == NULL) //NO EXISTING DIALOG
+		{
+			int err = osip_dialog_init_as_uac(&dialog, sip_msg);
+			if (err) {
+				printf("Can't osip_dialog_init_as_uac %d\n", err);
+			}
+			//my_application_add_existing_dialog(dialog);
+		}
+	}
+	dialog->your_instance = sip_client;
+
+	osip_message_t *ack_msg;
+	if (osip_message_init(&ack_msg)) {
+		OSIP_TRACE(osip_trace(__FILE__,__LINE__,OSIP_BUG,NULL,"Can't init message!\n"));
+		return;
+	}
+
+	osip_message_set_method(ack_msg, osip_strdup("ACK"));
+
+	osip_uri_init(&(ack_msg->req_uri));
+	osip_uri_set_scheme(ack_msg->req_uri, osip_strdup(SIP_URI_SCHEME));
+	osip_uri_set_username(ack_msg->req_uri, osip_strdup(to_str));
+	osip_uri_set_host(ack_msg->req_uri, osip_strdup(sip_client->dst_ip));
+
+	osip_message_set_version(ack_msg, osip_strdup(SIP_VERSION));
+
+	//osip_message_set_to(ack_msg, to->displayname);
+	//osip_message_set_from(ack_msg, from->displayname);
+	osip_from_clone(from, &ack_msg->from);
+	osip_to_clone(to, &ack_msg->to);
+
+	rc = osip_call_id_clone(sip_msg->call_id, &(ack_msg->call_id));
+	assert (rc == 0);
+
+	osip_cseq_t* cseq;
+	rc = osip_cseq_init(&cseq);
+	assert (rc == 0);
+
+	//char* seq_num_str = (char *)osip_malloc(11);
+	//sprintf(seq_num_str,"%i", atoi(sip_msg->cseq->number) + 1); // Hardcoded
+	osip_cseq_set_number(cseq, osip_strdup(sip_msg->cseq->number));
+	osip_cseq_set_method(cseq, osip_strdup("ACK"));
+	ack_msg->cseq = cseq;
+
+	osip_message_set_max_forwards(ack_msg, "70");
+
+	char tmp[MESSAGE_ENTRY_MAX_LENGTH];
+	snprintf(tmp, sizeof(tmp), "SIP/2.0/%s %s:%s;branch=z9hG4bK-%u",
+		 "TCP",
+		 //from->url->host,
+		 //from->url->port,
+		 "127.0.0.1",
+		 "5150",
+		 osip_build_random_number());
+
+	osip_message_set_via(ack_msg, tmp);
+
+	char src_port[7];
+	snprintf(src_port, sizeof(src_port), "%u", sip_client->src_port);
+	snprintf(tmp, sizeof(tmp), "<sip:ussd_sip@%s:%s>", sip_client->src_ip, src_port);
+	osip_message_set_contact(ack_msg, tmp);
+
+	snprintf(tmp, sizeof(tmp), "%i", EXPIRES_TIME_INSECS);
+	osip_message_set_expires(ack_msg, tmp);
+
+	osip_message_set_content_length(ack_msg, "0");
+	ack_msg->application_data = dialog;
+
+
+	int status;
+	osip_transaction_t *transaction;
+	status = osip_transaction_init(&transaction, ICT, osip, ack_msg);
+	if (status) {
+		printf("Failed to init transaction %d\n", status);
+		return ;
+	}
+
+	osip_transaction_set_your_instance(transaction, sip_client);
+	osip_transaction_set_reserved5(transaction, dialog);
+	osip_transaction_set_reserved6(transaction, osip);
+
+	osip_event_t *sip_event = osip_new_outgoing_sipmessage(ack_msg);
+	if (!sip_event) {
+		printf("Can't allocate message\n");
+		osip_message_free(ack_msg);
+		return ;
+	}
+
+	sip_event->transactionid = transaction->transactionid;
+	status = osip_message_force_update(ack_msg);
+	if (status) {
+		printf("Failed force update %d\n",status);
+		osip_message_free(ack_msg);
+		return;
+	}
+
+	status = osip_transaction_add_event(transaction, sip_event);
+	if (status) {
+		printf("Can't add event %d\n",status);
+		osip_message_free(ack_msg);
+		return;
+	}
+
+	osip_ict_execute(osip);
+//	osip_start_ack_retransmissions(transaction,
+//				       dialog, ack_msg, to->url->host,
+//				       (to->url->port) ? atoi(to->url->port) : 5060, -1);
+
+}
+
+void sip_cb_ict_rcv1xx(int type, osip_transaction_t *pott,osip_message_t *pomt)
+{
+	printf("OSIP_ICT_STATUS_1XX_RECEIVED\n");
+}
+void sip_cb_ict_rcv2xx_again(int type, osip_transaction_t *pott,osip_message_t *pomt)
+{
+	printf("OSIP_ICT_STATUS_2XX_RECEIVED_AGAIN\n");
+}
+void sip_cb_ict_rcv3456xx(int type, osip_transaction_t *pott,osip_message_t *pomt)
+{
+	printf("OSIP_ICT_STATUS_3456XX_RECEIVED\n");
+}
+
+void sip_cb_ict_kill_transaction(int type, osip_transaction_t *tr)
+{
+	printf("OSIP_ICT_KILL_TRANSACTION\n");
+	//int i = osip_remove_transaction (osip_transaction_get_reserved6(tr), tr);
+	//i = osip_transaction_free2(tr);
+	//if (i != 0) fprintf(stderr, "cannot remove transaction\n");
+	printf("KILLED TRANSACTION\n");
+}
+
+void cb_transport_error(int type, osip_transaction_t *a, int error)
+{
+	printf("OSIP_ICT_TRANSPORT_ERROR trnasaction: %p error: %d\n",
+	       a, error);
+}
+
 void sip_set_cbs(osip_t *osip)
 {
 	osip_set_cb_send_message(osip, sip_cb_send);
@@ -287,6 +472,23 @@ void sip_set_cbs(osip_t *osip)
 	osip_set_message_callback (osip, OSIP_NICT_STATUS_4XX_RECEIVED, sip_cb_rcv4xx);
 	osip_set_message_callback (osip, OSIP_NICT_STATUS_5XX_RECEIVED, sip_cb_rcv5xx);
 	osip_set_message_callback (osip, OSIP_NICT_STATUS_6XX_RECEIVED, sip_cb_rcv6xx);
+
+	osip_set_message_callback (osip, OSIP_ICT_STATUS_1XX_RECEIVED, sip_cb_ict_rcv1xx);
+	osip_set_message_callback (osip, OSIP_ICT_STATUS_2XX_RECEIVED, sip_cb_ict_rcv2xx);
+	osip_set_message_callback (osip, OSIP_ICT_STATUS_2XX_RECEIVED_AGAIN, sip_cb_ict_rcv2xx_again);
+	osip_set_message_callback (osip, OSIP_ICT_STATUS_3XX_RECEIVED, sip_cb_ict_rcv3456xx);
+	osip_set_message_callback (osip, OSIP_ICT_STATUS_4XX_RECEIVED, sip_cb_ict_rcv3456xx);
+	osip_set_message_callback (osip, OSIP_ICT_STATUS_5XX_RECEIVED, sip_cb_ict_rcv3456xx);
+	osip_set_message_callback (osip, OSIP_ICT_STATUS_6XX_RECEIVED, sip_cb_ict_rcv3456xx);
+
+	osip_set_kill_transaction_callback(osip, OSIP_ICT_KILL_TRANSACTION, sip_cb_ict_kill_transaction);
+
+	osip_set_transport_error_callback(osip, OSIP_ICT_TRANSPORT_ERROR, cb_transport_error);
+
+	//osip_set_kill_transaction_callback(osip ,OSIP_NIST_KILL_TRANSACTION, &cb_ist_kill_transaction);
+	//osip_set_kill_transaction_callback(osip ,OSIP_NICT_KILL_TRANSACTION, &cb_nict_kill_transaction);
+	//osip_set_kill_transaction_callback(osip ,OSIP_NIST_KILL_TRANSACTION, &cb_nist_kill_transaction);
+
 }
 
 
@@ -306,3 +508,107 @@ int sip_client_init(struct reg_proxy *reg, const char *src_ip, u_int16_t src_por
 
 	return 1;
 }
+
+
+
+// USSD part
+#define MAX_USSD_CONTENT 1000
+
+int tx_ss_handle(struct sip_client *sip_client, osip_t *osip, struct ss_request *ss,
+		 const char *extention)
+{
+	osip_message_t *reg_msg;
+
+	//static int seq_num = 1;
+	char *call_id_num = NULL;
+	char *seq_num_str = NULL;
+	osip_call_id_t *call_id;
+	char tmp[MESSAGE_ENTRY_MAX_LENGTH];
+	osip_cseq_t *cseq;
+	char src_port[6];
+
+	if (osip_message_init(&reg_msg)) {
+		OSIP_TRACE(osip_trace(__FILE__,__LINE__,OSIP_BUG,NULL,"Can't init message!\n"));
+		return -1;
+	}
+	osip_message_set_method(reg_msg, osip_strdup("INVITE"));
+
+	/////
+	osip_uri_init(&(reg_msg->req_uri));
+	osip_uri_set_scheme(reg_msg->req_uri, osip_strdup(SIP_URI_SCHEME));
+	osip_uri_set_username(reg_msg->req_uri, osip_strdup(ss->ussd_text));
+	osip_uri_set_host(reg_msg->req_uri, osip_strdup(sip_client->dst_ip));
+	osip_message_set_version(reg_msg, osip_strdup(SIP_VERSION));
+
+
+	sprintf(tmp, "<sip:%s@%s>", ss->ussd_text, sip_client->dst_ip);
+	osip_message_set_to(reg_msg, tmp);
+
+	sprintf(tmp, "<sip:%s@%s>;tag=%u", extention, sip_client->dst_ip, osip_build_random_number());
+	osip_message_set_from(reg_msg, tmp);
+
+	if (osip_call_id_init(&call_id)) {
+		OSIP_TRACE(osip_trace(__FILE__,__LINE__,OSIP_BUG,NULL,"call id failed!\n"));
+		osip_message_free(reg_msg);
+		return -1;
+	}
+
+	call_id_num = (char *)osip_malloc(MAX_ADDR_STR);
+	sprintf(call_id_num, "%u", osip_build_random_number());
+	osip_call_id_set_number(call_id, call_id_num);
+	reg_msg->call_id = call_id;
+
+	if (osip_cseq_init(&cseq)) {
+		OSIP_TRACE(osip_trace(__FILE__,__LINE__,OSIP_BUG,NULL,"seq init failed!\n"));
+		osip_message_free(reg_msg);
+		return -1;
+	}
+
+	seq_num_str = (char *)osip_malloc(11);
+	sprintf(seq_num_str,"%i", 1); // Hardcoded
+	osip_cseq_set_number(cseq, seq_num_str);
+	osip_cseq_set_method(cseq, osip_strdup("INVITE"));
+	reg_msg->cseq = cseq;
+
+	osip_message_set_max_forwards(reg_msg, "70");
+
+	snprintf(src_port, sizeof(src_port), "%u", sip_client->src_port);
+	snprintf(tmp, sizeof(tmp), "SIP/2.0/%s %s:%s;branch=z9hG4bK-%u", "TCP", sip_client->src_ip,
+					  src_port, osip_build_random_number());
+	osip_message_set_via(reg_msg, tmp);
+
+	snprintf(tmp, sizeof(tmp), "<sip:ussd_sip@%s:%s>", sip_client->src_ip, src_port);
+	osip_message_set_contact(reg_msg, tmp);
+
+	snprintf(tmp, sizeof(tmp), "%i", EXPIRES_TIME_INSECS);
+	osip_message_set_expires(reg_msg, tmp);
+
+	// Content
+	char content[MAX_USSD_CONTENT];
+	int content_len = snprintf(content, sizeof(content),
+				   "<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n"
+				   "<ussd-data>\n"
+				   " <language>%s</language>\n"
+				   " <ussd-string>%s</ussd-string>\n"
+				   "</ussd-data>",
+				   "en",
+				   ss->ussd_text);
+
+	snprintf(tmp, sizeof(tmp), "%i", content_len);
+	osip_message_set_content_length(reg_msg, tmp);
+	osip_message_set_content_type(reg_msg, "application/vnd.3gpp.ussd+xml");
+
+	osip_message_set_body(reg_msg, content, content_len);
+
+	//osip_message_set_allow(reg_msg, SIP_ALLOW);
+
+	printf("REG message ready, try to send\n");
+
+	if (sip_send(sip_client, osip, reg_msg, ICT)) {
+		printf("Error sending message!");
+		return -1;
+	}
+
+	return 0;
+
+}
diff --git a/openbsc/src/reg-proxy/sip_client.c b/openbsc/src/reg-proxy/sip_client.c
index c470fa08a..f161fa9df 100644
--- a/openbsc/src/reg-proxy/sip_client.c
+++ b/openbsc/src/reg-proxy/sip_client.c
@@ -247,11 +247,13 @@ void sip_client_destroy(struct sip_client *sip_client)
 int sip_client_send(struct sip_client *sip_client, struct msgb *msg)
 {
 	if (!sip_client) {
+		printf(" sip_client == NULL ");
 		msgb_free(msg);
 		return -ENOTCONN;
 	}
 
 	if (!sip_client->is_connected) {
+		printf(" !sip_client->is_connected ");
 		msgb_free(msg);
 		return -EAGAIN;
 	}
diff --git a/openbsc/src/reg-proxy/sup.c b/openbsc/src/reg-proxy/sup.c
index de08d94f9..a3f1263f2 100644
--- a/openbsc/src/reg-proxy/sup.c
+++ b/openbsc/src/reg-proxy/sup.c
@@ -4,6 +4,9 @@
 #include <openbsc/gprs_gsup_messages.h>
 #include <openbsc/sip.h>
 #include <openbsc/sup.h>
+#include <openbsc/gsm_04_08.h>
+#include <osmocom/gsm/gsm0480.h>
+#include <openbsc/ussd.h>
 
 static int handle_sup_upd_loc_req(struct gsm_sup_server *sup_server,
 									struct gprs_gsup_message *sup_msg)
@@ -20,6 +23,206 @@ static int handle_sup_upd_loc_req(struct gsm_sup_server *sup_server,
 	return rc;
 }
 
+static int handle_sup_ss(struct gsm_sup_server *sup_server,
+			 struct ss_request *ss,
+			 const char* extention)
+{
+	int rc = 0;
+	struct reg_proxy *reg = sup_server->app;
+	struct sip_client *sip_client = reg->sip_client;
+	osip_t *osip = reg->osip;
+
+	LOGP(DGPRS, LOGL_INFO,
+		"Try to send sip_register 0x%02x\n", ss->message_type);
+	rc = tx_ss_handle(sip_client, osip, ss, extention);
+	LOGP(DGPRS, LOGL_INFO,
+		"Sip_register was send 0x%02x\n", ss->message_type);
+	return rc;
+}
+
+
+enum {
+    FMAP_MSISDN        = 0x80
+};
+static int rx_uss_message_parse(struct ss_request *ss,
+				const uint8_t* data,
+				size_t len,
+				char* extention,
+				size_t extention_len)
+{
+	const uint8_t* const_data = data;
+
+	if (len < 1 + 2 + 3 + 3)
+		return -1;
+
+	/* skip GPRS_GSUP_MSGT_MAP */
+	ss->message_type = *(++const_data);
+	ss->component_type = *(++const_data);
+	const_data += 2;
+
+	//
+	if (*const_data != GSM0480_COMPIDTAG_INVOKE_ID) {
+		return -1;
+	}
+	const_data += 2;
+	ss->invoke_id = *const_data;
+	const_data++;
+
+	//
+	if (*const_data != GSM0480_OPERATION_CODE) {
+		return -1;
+	}
+	const_data += 2;
+	ss->opcode = *const_data;
+	const_data++;
+
+
+	while (const_data - data < len) {
+		uint8_t len;
+		switch (*const_data) {
+		case ASN1_OCTET_STRING_TAG:
+			len = *(++const_data);
+			strncpy((char*)ss->ussd_text,
+				(const char*)++const_data,
+				(len > MAX_LEN_USSD_STRING) ? MAX_LEN_USSD_STRING : len);
+			const_data += len;
+			break;
+
+		case FMAP_MSISDN:
+			len = *(++const_data);
+			gsm48_decode_bcd_number(extention,
+						extention_len,
+						const_data,
+						0);
+			const_data += len + 1;
+			break;
+		default:
+			DEBUGP(DMM, "Unknown code: %d\n", *const_data);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+static int subscr_uss_message(struct msgb *msg,
+			      struct ss_request *req,
+			      const char* extention)
+{
+	size_t bcd_len = 0;
+	uint8_t *gsup_indicator;
+
+	gsup_indicator = msgb_put(msg, 4);
+
+	/* First byte should always be GPRS_GSUP_MSGT_MAP */
+	gsup_indicator[0] = GPRS_GSUP_MSGT_MAP;
+	gsup_indicator[1] = req->message_type;
+	/* TODO ADD tid */
+	gsup_indicator[2] = req->component_type;
+
+	/* invokeId */
+	msgb_tlv_put(msg, GSM0480_COMPIDTAG_INVOKE_ID, 1, &req->invoke_id);
+
+	/* opCode */
+	msgb_tlv_put(msg, GSM0480_OPERATION_CODE, 1, &req->opcode);
+
+	if (req->ussd_text_len > 0) {
+		//msgb_tlv_put(msg, ASN1_OCTET_STRING_TAG, 1, &req->ussd_text_language);
+		msgb_tlv_put(msg, ASN1_OCTET_STRING_TAG, req->ussd_text_len, req->ussd_text);
+	}
+
+	if (extention) {
+		uint8_t bcd_buf[32];
+		bcd_len = gsm48_encode_bcd_number(bcd_buf, sizeof(bcd_buf), 0,
+						  extention);
+		msgb_tlv_put(msg, FMAP_MSISDN, bcd_len - 1, &bcd_buf[1]);
+	}
+
+	/* fill actual length */
+	gsup_indicator[3] = 3 + 3 + (req->ussd_text_len + 2) + (bcd_len + 2);
+
+	/* wrap with GSM0480_CTYPE_INVOKE */
+	// gsm0480_wrap_invoke(msg, req->opcode, invoke_id);
+	// gsup_indicator = msgb_push(msgb, 1);
+	// gsup_indicator[0] = GPRS_GSUP_MSGT_MAP;
+	return 0;
+}
+
+static int rx_sup_uss_message(struct gsm_sup_server *sup_server, const uint8_t* data, size_t len)
+{
+	char extention[32] = {0};
+	struct ss_request ss;
+	memset(&ss, 0, sizeof(ss));
+
+
+	if (rx_uss_message_parse(&ss, data, len, extention, sizeof(extention))) {
+		LOGP(DSUP, LOGL_ERROR, "Can't parse uss message\n");
+		return -1;
+	}
+
+	LOGP(DSUP, LOGL_ERROR, "Got mtype=0x%02x invoke_id=0x%02x opcode=0x%02x component_type=0x%02x text=%s\n",
+	     ss.message_type, ss.invoke_id, ss.opcode, ss.component_type, ss.ussd_text);
+
+	handle_sup_ss(sup_server, &ss, extention);
+
+	return 0;
+#if 0
+	if (strcmp((const char*)ss.ussd_text, "*#100#") == 0) {
+		ss.ussd_text_len = snprintf(ss.ussd_text,
+					    sizeof(ss.ussd_text),
+					    "Your extention is %s",
+					    extention);
+
+		/* Last message in the transaction */
+		ss.message_type = GSM0480_MTYPE_RELEASE_COMPLETE;
+		ss.component_type = GSM0480_CTYPE_RETURN_RESULT;
+	} else if (strcmp((const char*)ss.ussd_text, "*#101#") == 0) {
+		ss.ussd_text_len = snprintf(ss.ussd_text,
+					    sizeof(ss.ussd_text),
+					    "Select option:\n1) Option 1\n2) Option 2");
+
+		ss.message_type = GSM0480_MTYPE_FACILITY;
+		ss.component_type = GSM0480_CTYPE_INVOKE;
+		ss.opcode = GSM0480_OP_CODE_USS_REQUEST;
+	} else {
+		if (ss.component_type == GSM0480_CTYPE_RETURN_RESULT &&
+				ss.opcode == GSM0480_OP_CODE_USS_REQUEST) {
+			// Menu selected
+			char buffer[160];
+			strncpy(buffer, ss.ussd_text, sizeof(buffer));
+			ss.ussd_text_len = snprintf(ss.ussd_text,
+						    sizeof(ss.ussd_text),
+						    "You've selected \"%s\"",
+						    buffer);
+
+			ss.message_type = GSM0480_MTYPE_RELEASE_COMPLETE;
+			ss.component_type = GSM0480_CTYPE_RETURN_RESULT;
+			ss.opcode = GSM0480_OP_CODE_PROCESS_USS_REQ;
+		} else {
+			ss.ussd_text_len = 0;
+			ss.message_type = GSM0480_MTYPE_RELEASE_COMPLETE;
+			ss.component_type = GSM0480_CTYPE_REJECT;
+		}
+	}
+
+	if (ss.ussd_text_len > sizeof(ss.ussd_text))
+		ss.ussd_text_len = sizeof(ss.ussd_text);
+
+	struct msgb *msg = gprs_gsup_msgb_alloc();
+	subscr_uss_message(msg,
+			   &ss,
+			   (extention[0] == 0) ? NULL : extention);
+	LOGP(DGPRS, LOGL_INFO,
+	     "Sending USS, will send: %s\n", msgb_hexdump(msg));
+
+	if (!sup_server) {
+		msgb_free(msg);
+		return -ENOTSUP;
+	}
+	return sup_server_send(sup_server, msg);
+#endif
+}
+
 int rx_sup_message(struct gsm_sup_server *sup_server, struct msgb *msg)
 {
 	uint8_t *data = msgb_l2(msg);
@@ -29,6 +232,13 @@ int rx_sup_message(struct gsm_sup_server *sup_server, struct msgb *msg)
 	struct gprs_gsup_message sup_msg = {0};
 	//struct gsm_subscriber *subscr;
 
+    if (*data == GPRS_GSUP_MSGT_MAP) {
+	LOGP(DSUP, LOGL_INFO,
+		   "Receive USS: %s\n", msgb_hexdump(msg));
+
+	return rx_sup_uss_message(sup_server, data, data_len);
+    }
+
 	rc = gprs_gsup_decode(data, data_len, &sup_msg);
 	if (rc < 0) {
 		LOGP(DSUP, LOGL_ERROR,
diff --git a/openbsc/src/reg-proxy/test_sip.py b/openbsc/src/reg-proxy/test_sip.py
new file mode 100755
index 000000000..e8dae8c2f
--- /dev/null
+++ b/openbsc/src/reg-proxy/test_sip.py
@@ -0,0 +1,111 @@
+#!/usr/bin/python
+
+import sys
+import string
+import random
+import binascii
+import sip_parser
+import re
+from twisted.internet import defer
+from twisted.internet import protocol
+from twisted.python import log
+from twisted.protocols import sip
+from time import sleep
+
+TCP_SRC_IP   = "127.0.0.1"
+TCP_SRC_PORT = 5060
+
+class RegistrationProxyServer(protocol.Protocol):
+    src_ip = TCP_SRC_IP
+    src_port = TCP_SRC_PORT
+
+    def connectionMade(self):
+        self.ussd_queue = defer.DeferredQueue()
+        self.ussd_queue.get().addCallback(self.sipClientDataReceived)
+        from twisted.internet import reactor
+        #reactor.listenTCP(UDP_TCP_PORT, self.sip_client_factory)
+
+    def sipClientDataReceived(self, data):
+        log.msg("\n[USSD:RX]\n%s" % data)
+        if data:
+          msgType, firstLine, headers, body = sip_parser.parseSipMessage(data)
+          via = headers["via"][0].split(";")
+          via_branch = via[1].split("=")
+          from_hdr = headers["from"].split(";")
+          from_tag = from_hdr[1]
+          to_hdr = headers["to"].split(";")
+          to_tag = from_hdr[1]
+          call_id = headers["call-id"]
+          sip_url = re.split(r"[:@]", from_hdr[0])
+          ussd_url = sip_url[1]
+          contact = headers["contact"].split("@")
+          cseq = headers["cseq"]
+          via_dest_ip,via_dest_port=via[0].split(" ")[1].split(":")
+
+          if msgType=="INVITE":
+              #r = sip.Response(100, "Trying")
+              #r.addHeader('Via', sip.Via(via_dest_ip, via_dest_port, transport='TCP', ttl=None, hidden=False, received=None, rport=None, branch=via_branch[1], maddr=None).toString())
+              #r.addHeader('From', from_hdr[0]) #"<sip:%s@%s>;%s" % (from_hdr[0], self.src_ip, from_tag))
+              #r.addHeader('To', to_hdr[0]) #"<sip:%s@%s>;%s" % (to_hdr[0], self.src_ip, to_tag))
+              #r.addHeader('Call-Id', call_id)
+              #r.addHeader('Max-Forwards', 20)
+              #r.addHeader('Cseq',  cseq)
+              #r.addHeader('Contact',  '<sip:test@127.0.0.1:5060>')
+              #r.addHeader('Content-Length', 0)
+              #r.addHeader("Authentication-Info", auth_info)
+              #log.msg("\n[SIP:TX]\n%s" % r.toString())
+              #self.transport.write(r.toString())
+
+              #sleep(5)
+
+              r = sip.Response(200, "OK")
+              r.addHeader('Via', sip.Via(via_dest_ip, via_dest_port, transport='TCP', ttl=None, hidden=False, received=None, rport=None, branch=via_branch[1], maddr=None).toString())
+              r.addHeader('From', "%s;%s" % (from_hdr[0], from_tag)) #"<sip:%s@%s>;%s" % (from_hdr[0], self.src_ip, from_tag))
+              r.addHeader('To', "%s;%s" % (to_hdr[0], to_tag)) #"<sip:%s@%s>;%s" % (to_hdr[0], self.src_ip, to_tag))
+              r.addHeader('Call-Id', call_id)
+              r.addHeader('Max-Forwards', 20)
+              r.addHeader('Cseq',  cseq)
+              r.addHeader('Contact',  '<sip:test@127.0.0.1:5060>')
+              r.addHeader('Recv-Info', 'g.3gpp.ussd')
+              r.addHeader('Content-Length', 0)
+              #r.addHeader("Authentication-Info", auth_info)
+              log.msg("\n[SIP:TX]\n%s" % r.toString())
+              self.transport.write(r.toString())
+          elif msgType=="ACK":
+              msg = "<?xml version=\"1.0\" encoding=\"UTF-8\"?><ussd-data><language>en</language><ussd-string>%s</ussd-string></ussd-data>" % (
+                                "Test");
+
+              r = sip.Request("BYE", to_hdr[0].replace('<', '').replace('>', ''))
+              r.addHeader('Via', sip.Via(via_dest_ip, via_dest_port, transport='TCP', ttl=None, hidden=False, received=None, rport=None, branch=via_branch[1], maddr=None).toString())
+              r.addHeader('From', "<sip:%s@%s>;%s" % (ussd_url, self.src_ip, from_tag))
+              r.addHeader('To', "<sip:%s@%s>;%s" % (ussd_url, self.src_ip, to_tag))
+              r.addHeader('Call-Id', call_id)
+              r.addHeader('Max-Forwards', 20)
+              r.addHeader('Cseq',  "%d BYE" % int(cseq.split(' ')[0]) + 1)
+              r.addHeader('Recv-Info', 'g.3gpp.ussd')
+              r.addHeader('Content-Type', 'application/vnd.3gpp.ussd+xml')
+              r.addHeader('Content-Disposition', 'Info-Package')
+              r.addHeader('Content-Length', msg.len)
+              #r.addHeader("Authentication-Info", auth_info)
+              log.msg("\n[SIP:TX]]\n%s" % r.toString())
+              self.transport.write(r.toString() + "\n" + msg)
+          else:
+              sys.exit(-1)
+
+          #self.ussd_queue.get().addCallback(self.sipClientDataReceived)
+
+    def dataReceived(self, data):
+          #log.msg("\n[IMSI:RX] [Proxy <=============== BSC]\n%s" % data)
+          self.ussd_queue.put(data)
+
+class RegistrationProxyServerFactory(protocol.ClientFactory):
+    protocol = RegistrationProxyServer
+
+if __name__ == "__main__":
+    log.startLogging(sys.stdout)
+    factory = RegistrationProxyServerFactory()
+    from twisted.internet import reactor
+    reactor.listenTCP(TCP_SRC_PORT, factory)
+    reactor.run()
+
+