Initial support of SMPP interface for MT-SMS
diff --git a/openbsc/configure.ac b/openbsc/configure.ac
index 86dab55..279d12a 100644
--- a/openbsc/configure.ac
+++ b/openbsc/configure.ac
@@ -46,6 +46,7 @@
 PKG_CHECK_MODULES(LIBOSMOGSM, libosmogsm >= 0.3.0)
 PKG_CHECK_MODULES(LIBOSMOABIS, libosmoabis >= 0.1.0)
 PKG_CHECK_MODULES(LIBOSMOGB, libosmogb >= 0.5.2)
+PKG_CHECK_MODULES(LIBSMPP34, libsmpp34 >= 1.10)
 
 found_libgtp=yes
 PKG_CHECK_MODULES(LIBGTP, libgtp, , found_libgtp=no)
diff --git a/openbsc/include/openbsc/debug.h b/openbsc/include/openbsc/debug.h
index 0023b8c..84931af 100644
--- a/openbsc/include/openbsc/debug.h
+++ b/openbsc/include/openbsc/debug.h
@@ -32,6 +32,7 @@
 	DSNDCP,
 	DNAT,
 	DCTRL,
+	DSMPP,
 	Debug_LastEntry,
 };
 
diff --git a/openbsc/src/libcommon/debug.c b/openbsc/src/libcommon/debug.c
index fad5e27..d082492 100644
--- a/openbsc/src/libcommon/debug.c
+++ b/openbsc/src/libcommon/debug.c
@@ -161,6 +161,11 @@
 		.description = "Control interface",
 		.enabled = 1, .loglevel = LOGL_NOTICE,
 	},
+	[DSMPP] = {
+		.name = "DSMPP",
+		.description = "SMPP interface for external SMS apps",
+		.enabled = 1, .loglevel = LOGL_DEBUG,
+	},
 };
 
 enum log_filter {
diff --git a/openbsc/src/libmsc/Makefile.am b/openbsc/src/libmsc/Makefile.am
index 4a1488d..c29d521 100644
--- a/openbsc/src/libmsc/Makefile.am
+++ b/openbsc/src/libmsc/Makefile.am
@@ -16,5 +16,6 @@
 			ussd.c \
 			vty_interface_layer3.c \
 			transaction.c \
+			smpp_smsc.c smpp_openbsc.c \
 			osmo_msc.c
 
diff --git a/openbsc/src/libmsc/smpp_openbsc.c b/openbsc/src/libmsc/smpp_openbsc.c
new file mode 100644
index 0000000..cd3ab3a
--- /dev/null
+++ b/openbsc/src/libmsc/smpp_openbsc.c
@@ -0,0 +1,206 @@
+/* OpenBSC SMPP 3.4 interface, SMSC-side implementation */
+
+/* (C) 2012 by Harald Welte <laforge@gnumonks.org>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdint.h>
+#include <errno.h>
+
+#include <smpp34.h>
+#include <smpp34_structs.h>
+#include <smpp34_params.h>
+
+#include <osmocom/core/utils.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/gsm/protocol/gsm_04_11.h>
+
+#include <openbsc/gsm_subscriber.h>
+#include <openbsc/debug.h>
+#include <openbsc/db.h>
+#include <openbsc/gsm_04_11.h>
+
+#include "smpp_smsc.h"
+
+
+static struct gsm_subscriber *subscr_by_dst(struct gsm_network *net,
+					    uint8_t npi, uint8_t ton, const char *addr)
+{
+	struct gsm_subscriber *subscr = NULL;
+
+	switch (npi) {
+	case NPI_Land_Mobile_E212:
+		subscr = subscr_get_by_imsi(net, addr);
+		break;
+	case NPI_ISDN_E163_E164:
+	case NPI_Private:
+		subscr = subscr_get_by_extension(net, addr);
+		break;
+	default:
+		LOGP(DSMPP, LOGL_NOTICE, "Unsupported NPI: %u\n", npi);
+		break;
+	}
+
+	return subscr;
+}
+
+struct tlv_t *find_tlv(struct tlv_t *head, uint16_t tag)
+{
+	struct tlv_t *t;
+
+	for (t = head; t != NULL; t = t->next) {
+		if (t->tag == tag)
+			return t;
+	}
+	return NULL;
+}
+
+/* convert from submit_sm_t to gsm_sms */
+static int submit_to_sms(struct gsm_sms **psms, struct gsm_network *net,
+			 const struct submit_sm_t *submit)
+{
+	struct gsm_subscriber *dest;
+	struct gsm_sms *sms;
+	struct tlv_t *t;
+	const uint8_t *sms_msg;
+	unsigned int sms_msg_len;
+
+	dest = subscr_by_dst(net, submit->dest_addr_npi,
+			     submit->dest_addr_ton,
+			     (const char *)submit->destination_addr);
+	if (!dest) {
+		LOGP(DSMS, LOGL_NOTICE, "SMPP SUBMIT-SM for unknown subscriber: "
+		     "%s (NPI=%u)\n", submit->destination_addr,
+		     submit->dest_addr_npi);
+		return ESME_RINVDSTADR;
+	}
+
+	t = find_tlv(submit->tlv, TLVID_message_payload);
+	if (t) {
+		if (submit->sm_length) {
+			/* ERROR: we cannot have botH! */
+			return ESME_ROPTPARNOTALLWD;
+		}
+		sms_msg = t->value.octet;
+		sms_msg_len = t->length;
+	} else if (submit->short_message && submit->sm_length) {
+		sms_msg = submit->short_message;
+		sms_msg_len = submit->sm_length;
+	} else {
+		sms_msg = NULL;
+		sms_msg_len = 0;
+	}
+
+	sms = sms_alloc();
+	sms->receiver = subscr_get(dest);
+	strncpy(sms->dest_addr, dest->extension, sizeof(sms->dest_addr)-1);
+	sms->sender = subscr_get_by_id(net, 1);
+
+	if (submit->esm_class & 0x40)
+		sms->ud_hdr_ind = 1;
+
+	if (submit->esm_class & 0x80) {
+		sms->reply_path_req = 1;
+#warning Implement reply path
+	}
+
+	switch (submit->data_coding) {
+	case 0x00:
+	case 0x01: /* GSM default alphabet */
+		sms->data_coding_scheme = GSM338_DCS_1111_7BIT;
+		strncpy(sms->text, (char *)sms_msg,
+			OSMO_MIN(sizeof(sms->text)-1, sms_msg_len));
+		sms->user_data_len = gsm_7bit_encode(sms->user_data, sms->text);
+		break;
+	case 0x02:
+	case 0x04: /* 8-bit binary */
+		sms->data_coding_scheme = GSM338_DCS_1111_8BIT_DATA;
+		memcpy(sms->user_data, sms_msg, sms_msg_len);
+		sms->user_data_len = sms_msg_len;
+		break;
+	case 0x80: /* UCS-2 */
+		sms->data_coding_scheme = (2 << 2);
+		memcpy(sms->user_data, sms_msg, submit->sm_length);
+		sms->user_data_len = sms_msg_len;
+		break;
+		/* FIXME */
+	default:
+		sms_free(sms);
+		return ESME_RUNKNOWNERR;
+	}
+
+	*psms = sms;
+	return ESME_ROK;
+}
+
+int handle_smpp_submit(struct osmo_esme *esme, struct submit_sm_t *submit,
+		       struct submit_sm_resp_t *submit_r)
+{
+	struct gsm_sms *sms;
+	int rc = -1;
+
+	rc = submit_to_sms(&sms, esme->smsc->priv, submit);
+	if (rc != ESME_ROK) {
+		submit_r->command_status = rc;
+		return 0;
+	}
+	/* FIXME: TP-PID */
+
+	switch (submit->esm_class & 3) {
+	case 0: /* default */
+	case 1: /* datagram */
+	case 3: /* store-and-forward */
+		rc = db_sms_store(sms);
+		if (rc < 0) {
+			LOGP(DSMS, LOGL_ERROR, "SMPP SUBMIT-SM: Unable to "
+				"store SMS in database\n");
+			sms_free(sms);
+			submit_r->command_status = ESME_RSYSERR;
+			return 0;
+		}
+		strcpy((char *)submit_r->message_id, "msg_id_not_implemented");
+		LOGP(DSMS, LOGL_INFO, "SMPP SUBMIT-SM: Stored in DB\n");
+		rc = 0;
+		break;
+	case 2: /* forward (i.e. transaction) mode */
+		/* FIXME */
+		rc = 1; /* don't send any response yet */
+		break;
+	}
+	return rc;
+}
+
+
+int smpp_openbsc_init(struct gsm_network *net, uint16_t port)
+{
+	struct smsc *smsc = talloc_zero(net, struct smsc);
+	int rc;
+
+	smsc->priv = net;
+
+	rc = smpp_smsc_init(smsc, port);
+	if (rc < 0)
+		talloc_free(smsc);
+
+	return rc;
+}
+
diff --git a/openbsc/src/libmsc/smpp_smsc.c b/openbsc/src/libmsc/smpp_smsc.c
new file mode 100644
index 0000000..ff6b1f9
--- /dev/null
+++ b/openbsc/src/libmsc/smpp_smsc.c
@@ -0,0 +1,506 @@
+/* SMPP 3.4 interface, SMSC-side implementation */
+/* (C) 2012 by Harald Welte <laforge@gnumonks.org>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdint.h>
+#include <errno.h>
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+#include <smpp34.h>
+#include <smpp34_structs.h>
+#include <smpp34_params.h>
+
+#include <osmocom/core/utils.h>
+#include <osmocom/core/socket.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/write_queue.h>
+#include <osmocom/core/talloc.h>
+
+#include "smpp_smsc.h"
+
+#include <openbsc/debug.h>
+
+enum emse_bind {
+	ESME_BIND_RX = 0x01,
+	ESME_BIND_TX = 0x02,
+};
+
+
+
+#define INIT_RESP(type, resp, req) 		{ \
+	memset((resp), 0, sizeof(*(resp)));	  \
+	(resp)->command_length	= 0;		  \
+	(resp)->command_id	= type;		  \
+	(resp)->command_status	= ESME_ROK;	  \
+	(resp)->sequence_number	= (req)->sequence_number;	\
+}
+
+#define PACK_AND_SEND(esme, ptr)	pack_and_send(esme, (ptr)->command_id, ptr)
+static int pack_and_send(struct osmo_esme *esme, uint32_t type, void *ptr)
+{
+	struct msgb *msg = msgb_alloc(4096, "SMPP_Tx");
+	int rc, rlen;
+	if (!msg)
+		return -ENOMEM;
+
+	rc = smpp34_pack(type, msg->tail, msgb_tailroom(msg), &rlen, ptr);
+	if (rc != 0) {
+		LOGP(DSMPP, LOGL_ERROR, "Error during smpp34_pack(): %s\n",
+		     smpp34_strerror);
+		msgb_free(msg);
+		return -EINVAL;
+	}
+	msgb_put(msg, rlen);
+
+	return osmo_wqueue_enqueue(&esme->wqueue, msg);
+}
+
+static int smpp_tx_gen_nack(struct osmo_esme *esme, uint32_t seq, uint32_t status)
+{
+	struct generic_nack_t nack;
+
+	nack.command_length = 0;
+	nack.command_id = GENERIC_NACK;
+	nack.sequence_number = seq;
+	nack.command_status = status;
+
+	return PACK_AND_SEND(esme, &nack);
+}
+
+static inline uint32_t smpp_msgb_cmdid(struct msgb *msg)
+{
+	uint8_t *tmp = msgb_data(msg) + 4;
+	return ntohl(*(uint32_t *)tmp);
+}
+
+static inline uint32_t smpp_msgb_seq(struct msgb *msg)
+{
+	uint8_t *tmp = msgb_data(msg);
+	return ntohl(*(uint32_t *)tmp);
+}
+
+
+static int smpp_handle_gen_nack(struct osmo_esme *esme, struct msgb *msg)
+{
+	struct generic_nack_t nack;
+	char buf[SMALL_BUFF];
+	int rc;
+
+	rc = smpp34_unpack(GENERIC_NACK, &nack, msgb_data(msg),
+			   msgb_length(msg));
+	if (rc < 0)
+		return rc;
+
+	LOGP(DSMPP, LOGL_ERROR, "%s: GENERIC NACK: %s\n", esme->system_id,
+	     str_command_status(nack.command_status, buf));
+
+	return 0;
+}
+
+static int smpp_handle_bind_rx(struct osmo_esme *esme, struct msgb *msg)
+{
+	struct bind_receiver_t bind;
+	struct bind_receiver_resp_t bind_r;
+	int rc;
+
+	rc = smpp34_unpack(BIND_RECEIVER, &bind, msgb_data(msg),
+			   msgb_length(msg));
+	if (rc < 0)
+		return rc;
+
+	INIT_RESP(BIND_TRANSMITTER_RESP, &bind_r, &bind);
+
+	LOGP(DSMPP, LOGL_INFO, "%s: BIND Rx from (Version %02x)\n",
+		bind.system_id, bind.interface_version);
+
+	if (bind.interface_version != SMPP_VERSION) {
+		bind_r.command_status = ESME_RSYSERR;
+		goto err;
+	}
+
+	if (esme->bind_flags) {
+		bind_r.command_status = ESME_RALYBND;
+		goto err;
+	}
+
+	esme->smpp_version = bind.interface_version;
+	snprintf(esme->system_id, sizeof(esme->system_id), "%s",
+		 bind.system_id);
+	esme->bind_flags = ESME_BIND_RX;
+
+	/* FIXME */
+err:
+	return 0;
+}
+
+static int smpp_handle_bind_tx(struct osmo_esme *esme, struct msgb *msg)
+{
+	struct bind_transmitter_t bind;
+	struct bind_transmitter_resp_t bind_r;
+	struct tlv_t tlv;
+	int rc;
+
+	rc = smpp34_unpack(BIND_TRANSMITTER, &bind, msgb_data(msg),
+			   msgb_length(msg));
+	if (rc < 0) {
+		printf("error during unpack: %s\n", smpp34_strerror);
+		return rc;
+	}
+
+	INIT_RESP(BIND_TRANSMITTER_RESP, &bind_r, &bind);
+
+	LOGP(DSMPP, LOGL_INFO, "%s: BIND Tx (Version %02x)\n",
+		bind.system_id, bind.interface_version);
+
+	if (bind.interface_version != SMPP_VERSION) {
+		bind_r.command_status = ESME_RSYSERR;
+		goto err;
+	}
+
+	if (esme->bind_flags) {
+		bind_r.command_status = ESME_RALYBND;
+		goto err;
+	}
+
+	esme->smpp_version = bind.interface_version;
+	snprintf(esme->system_id, sizeof(esme->system_id), "%s", bind.system_id);
+	esme->bind_flags = ESME_BIND_TX;
+
+	/* build response */
+	snprintf((char *)bind_r.system_id, sizeof(bind_r.system_id), "%s",
+		 esme->smsc->system_id);
+
+	/* add interface version TLV */
+	tlv.tag = TLVID_sc_interface_version;
+	tlv.length = sizeof(uint8_t);
+	tlv.value.val16 = esme->smpp_version;
+	build_tlv(&bind_r.tlv, &tlv);
+
+err:
+	return PACK_AND_SEND(esme, &bind_r);
+}
+
+static int smpp_handle_bind_trx(struct osmo_esme *esme, struct msgb *msg)
+{
+	struct bind_transceiver_t bind;
+	struct bind_transceiver_resp_t bind_r;
+	int rc;
+
+	rc = smpp34_unpack(BIND_TRANSCEIVER, &bind, msgb_data(msg),
+			   msgb_length(msg));
+	if (rc < 0)
+		return rc;
+
+	INIT_RESP(BIND_TRANSMITTER_RESP, &bind_r, &bind);
+
+	LOGP(DSMPP, LOGL_INFO, "%s: BIND Trx (Version %02x)\n",
+		bind.system_id, bind.interface_version);
+
+	if (bind.interface_version != SMPP_VERSION) {
+		bind_r.command_status = ESME_RSYSERR;
+		goto err;
+	}
+
+	if (esme->bind_flags) {
+		bind_r.command_status = ESME_RALYBND;
+		goto err;
+	}
+
+	esme->smpp_version = bind.interface_version;
+	snprintf(esme->system_id, sizeof(esme->system_id), "%s", bind.system_id);
+	esme->bind_flags |= ESME_BIND_TX | ESME_BIND_RX;
+
+	/* FIXME */
+err:
+	return 0;
+}
+
+static int smpp_handle_unbind(struct osmo_esme *esme, struct msgb *msg)
+{
+	struct unbind_t unbind;
+	struct unbind_resp_t unbind_r;
+	int rc;
+
+	rc = smpp34_unpack(UNBIND, &unbind, msgb_data(msg),
+			   msgb_length(msg));
+	if (rc < 0)
+		return rc;
+
+	INIT_RESP(UNBIND_RESP, &unbind_r, &unbind);
+
+	LOGP(DSMPP, LOGL_INFO, "%s: UNBIND\n", esme->system_id);
+
+	if (esme->bind_flags == 0) {
+		unbind_r.command_status = ESME_RINVBNDSTS;
+		goto err;
+	}
+
+	esme->bind_flags = 0;
+err:
+	return PACK_AND_SEND(esme, &unbind_r);
+}
+
+
+static int smpp_handle_enq_link(struct osmo_esme *esme, struct msgb *msg)
+{
+	struct enquire_link_t enq;
+	struct enquire_link_resp_t enq_r;
+	int rc;
+
+	rc = smpp34_unpack(ENQUIRE_LINK, &enq, msgb_data(msg),
+			   msgb_length(msg));
+	if (rc < 0)
+		return rc;
+
+	LOGP(DSMPP, LOGL_DEBUG, "%s: Enquire Link\n", esme->system_id);
+
+	INIT_RESP(ENQUIRE_LINK_RESP, &enq_r, &enq);
+
+	return PACK_AND_SEND(esme, &enq_r);
+}
+
+static int smpp_handle_submit(struct osmo_esme *esme, struct msgb *msg)
+{
+	struct submit_sm_t submit;
+	struct submit_sm_resp_t submit_r;
+	int rc;
+
+	rc = smpp34_unpack(SUBMIT_SM, &submit, msgb_data(msg),
+			   msgb_length(msg));
+	if (rc < 0)
+		return rc;
+
+	INIT_RESP(SUBMIT_SM_RESP, &submit_r, &submit);
+
+	if (!(esme->bind_flags & ESME_BIND_TX)) {
+		submit_r.command_status = ESME_RINVBNDSTS;
+		return PACK_AND_SEND(esme, &submit_r);
+	}
+
+	LOGP(DSMPP, LOGL_INFO, "%s: SUBMIT-SM(%s)\n", esme->system_id,
+	     submit.service_type);
+
+	INIT_RESP(SUBMIT_SM_RESP, &submit_r, &submit);
+
+	rc = handle_smpp_submit(esme, &submit, &submit_r);
+	if (rc == 0)
+		return PACK_AND_SEND(esme, &submit_r);
+
+	return rc;
+}
+
+/* one complete SMPP PDU from the ESME has been received */
+static int smpp_pdu_rx(struct osmo_esme *esme, struct msgb *msg)
+{
+	uint32_t cmd_id = smpp_msgb_cmdid(msg);
+	int rc = 0;
+
+	LOGP(DSMPP, LOGL_DEBUG, "%s: smpp_pdu_rx(%s)\n", esme->system_id,
+	     osmo_hexdump(msgb_data(msg), msgb_length(msg)));
+
+	switch (cmd_id) {
+	case GENERIC_NACK:
+		rc = smpp_handle_gen_nack(esme, msg);
+		break;
+	case BIND_RECEIVER:
+		rc = smpp_handle_bind_rx(esme, msg);
+		break;
+	case BIND_TRANSMITTER:
+		rc = smpp_handle_bind_tx(esme, msg);
+		break;
+	case BIND_TRANSCEIVER:
+		rc = smpp_handle_bind_trx(esme, msg);
+		break;
+	case UNBIND:
+		rc = smpp_handle_unbind(esme, msg);
+		break;
+	case ENQUIRE_LINK:
+		rc = smpp_handle_enq_link(esme, msg);
+		break;
+	case SUBMIT_SM:
+		rc = smpp_handle_submit(esme, msg);
+		break;
+	case DELIVER_SM:
+		break;
+	case DATA_SM:
+		break;
+	case CANCEL_SM:
+	case QUERY_SM:
+	case REPLACE_SM:
+	case SUBMIT_MULTI:
+		LOGP(DSMPP, LOGL_NOTICE, "%s: Unimplemented PDU Commmand "
+		     "0x%08x\n", esme->system_id, cmd_id);
+		break;
+	default:
+		LOGP(DSMPP, LOGL_ERROR, "%s: Unknown PDU Command 0x%08x\n",
+		     esme->system_id, cmd_id);
+		rc = smpp_tx_gen_nack(esme, smpp_msgb_seq(msg), ESME_RINVCMDID);
+		break;
+	}
+
+	return rc;
+}
+
+static void esme_destroy(struct osmo_esme *esme)
+{
+	osmo_wqueue_clear(&esme->wqueue);
+	osmo_fd_unregister(&esme->wqueue.bfd);
+	close(esme->wqueue.bfd.fd);
+	llist_del(&esme->list);
+	talloc_free(esme);
+}
+
+/* call-back when per-ESME TCP socket has some data to be read */
+static int esme_link_read_cb(struct osmo_fd *ofd)
+{
+	struct osmo_esme *esme = ofd->data;
+	uint32_t len;
+	uint8_t *lenptr = (uint8_t *) &len;
+	uint8_t *cur;
+	struct msgb *msg;
+	int rdlen;
+	int rc;
+
+	switch (esme->read_state) {
+	case READ_ST_IN_LEN:
+		rdlen = sizeof(uint32_t) - esme->read_idx;
+		rc = read(ofd->fd, lenptr + esme->read_idx, rdlen);
+		if (rc < 0) {
+			LOGP(DSMPP, LOGL_ERROR, "read returned %d\n", rc);
+		} else if (rc == 0) {
+			goto dead_socket;
+		} else
+			esme->read_idx += rc;
+		if (esme->read_idx >= sizeof(uint32_t)) {
+			esme->read_len = ntohl(len);
+			msg = msgb_alloc(esme->read_len, "SMPP Rx");
+			if (!msg)
+				return -ENOMEM;
+			esme->read_msg = msg;
+			cur = msgb_put(msg, sizeof(uint32_t));
+			memcpy(cur, lenptr, sizeof(uint32_t));
+			esme->read_state = READ_ST_IN_MSG;
+			esme->read_idx = sizeof(uint32_t);
+		}
+		break;
+	case READ_ST_IN_MSG:
+		msg = esme->read_msg;
+		rdlen = esme->read_len - esme->read_idx;
+		rc = read(ofd->fd, msg->tail, OSMO_MIN(rdlen, msgb_tailroom(msg)));
+		if (rc < 0) {
+			LOGP(DSMPP, LOGL_ERROR, "read returned %d\n", rc);
+		} else if (rc == 0) {
+			goto dead_socket;
+		} else {
+			esme->read_idx += rc;
+			msgb_put(msg, rc);
+		}
+
+		if (esme->read_idx >= esme->read_len) {
+			rc = smpp_pdu_rx(esme, esme->read_msg);
+			esme->read_msg = NULL;
+			esme->read_idx = 0;
+			esme->read_len = 0;
+			esme->read_state = READ_ST_IN_LEN;
+		}
+		break;
+	}
+
+	return 0;
+dead_socket:
+	msgb_free(esme->read_msg);
+	esme_destroy(esme);
+
+	return 0;
+}
+
+/* call-back of write queue once it wishes to write a message to the socket */
+static void esme_link_write_cb(struct osmo_fd *ofd, struct msgb *msg)
+{
+	struct osmo_esme *esme = ofd->data;
+	int rc;
+
+	rc = write(ofd->fd, msgb_data(msg), msgb_length(msg));
+	if (rc == 0) {
+		esme_destroy(esme);
+	} else if (rc < msgb_length(msg)) {
+		LOGP(DSMPP, LOGL_ERROR, "%s: Short write\n", esme->system_id);
+		return;
+	}
+}
+
+/* callback for already-accepted new TCP socket */
+static int link_accept_cb(struct smsc *smsc, int fd,
+			  struct sockaddr_storage *s, socklen_t s_len)
+{
+	struct osmo_esme *esme = talloc_zero(smsc, struct osmo_esme);
+	if (!esme)
+		return -ENOMEM;
+
+	esme->smsc = smsc;
+	osmo_wqueue_init(&esme->wqueue, 10);
+	esme->wqueue.bfd.fd = fd;
+	esme->wqueue.bfd.data = esme;
+	esme->wqueue.bfd.when = BSC_FD_READ;
+	osmo_fd_register(&esme->wqueue.bfd);
+
+	esme->wqueue.read_cb = esme_link_read_cb;
+	esme->wqueue.write_cb = esme_link_write_cb;
+
+	esme->sa_len = OSMO_MIN(sizeof(esme->sa), s_len);
+	memcpy(&esme->sa, s, esme->sa_len);
+
+	llist_add_tail(&esme->list, &smsc->esme_list);
+
+	return 0;
+}
+
+/* callback of listening TCP socket */
+static int smsc_fd_cb(struct osmo_fd *ofd, unsigned int what)
+{
+	int rc;
+	struct sockaddr_storage sa;
+	socklen_t sa_len = sizeof(sa);
+
+	rc = accept(ofd->fd, (struct sockaddr *)&sa, &sa_len);
+	if (rc < 0) {
+		LOGP(DSMPP, LOGL_ERROR, "Accept returns %d (%s)\n",
+		     rc, strerror(errno));
+		return rc;
+	}
+	return link_accept_cb(ofd->data, rc, &sa, sa_len);
+}
+
+int smpp_smsc_init(struct smsc *smsc, uint16_t port)
+{
+	int rc;
+
+	INIT_LLIST_HEAD(&smsc->esme_list);
+	smsc->listen_ofd.data = smsc;
+	smsc->listen_ofd.cb = smsc_fd_cb;
+	rc = osmo_sock_init_ofd(&smsc->listen_ofd, AF_UNSPEC, SOCK_STREAM,
+				IPPROTO_TCP, NULL, port, OSMO_SOCK_F_BIND);
+
+	return rc;
+}
diff --git a/openbsc/src/libmsc/smpp_smsc.h b/openbsc/src/libmsc/smpp_smsc.h
new file mode 100644
index 0000000..500fbc3
--- /dev/null
+++ b/openbsc/src/libmsc/smpp_smsc.h
@@ -0,0 +1,52 @@
+#ifndef _SMPP_SMSC_H
+#define _SMPP_SMSC_H
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+#include <osmocom/core/utils.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/write_queue.h>
+
+#include <smpp34.h>
+#include <smpp34_structs.h>
+#include <smpp34_params.h>
+
+enum esme_read_state {
+	READ_ST_IN_LEN = 0,
+	READ_ST_IN_MSG = 1,
+};
+
+struct osmo_esme {
+	struct llist_head list;
+	struct smsc *smsc;
+
+	struct osmo_wqueue wqueue;
+	struct sockaddr_storage sa;
+	socklen_t sa_len;
+
+	enum esme_read_state read_state;
+	uint32_t read_len;
+	uint32_t read_idx;
+	struct msgb *read_msg;
+
+	uint8_t smpp_version;
+	char system_id[16+1];
+
+	uint8_t bind_flags;
+};
+
+struct smsc {
+	struct osmo_fd listen_ofd;
+	struct llist_head esme_list;
+	char system_id[16+1];
+	void *priv;
+};
+
+
+int smpp_smsc_init(struct smsc *smsc, uint16_t port);
+
+int handle_smpp_submit(struct osmo_esme *esme, struct submit_sm_t *submit,
+			struct submit_sm_resp_t *submit_r);
+
+#endif
diff --git a/openbsc/src/osmo-nitb/Makefile.am b/openbsc/src/osmo-nitb/Makefile.am
index bfea9f8..dc69232 100644
--- a/openbsc/src/osmo-nitb/Makefile.am
+++ b/openbsc/src/osmo-nitb/Makefile.am
@@ -1,7 +1,7 @@
 INCLUDES = $(all_includes) -I$(top_srcdir)/include -I$(top_builddir)
 AM_CFLAGS=-Wall	$(COVERAGE_CFLAGS)			\
   	$(LIBOSMOCORE_CFLAGS) $(LIBOSMOGSM_CFLAGS) $(LIBOSMOVTY_CFLAGS) \
-	$(LIBOSMOABIS_CFLAGS)
+	$(LIBOSMOABIS_CFLAGS) $(LIBSMPP34_CFLAGS)
 
 AM_LDFLAGS = $(COVERAGE_LDFLAGS)
 
@@ -17,4 +17,4 @@
 		$(top_builddir)/src/libcommon/libcommon.a \
 		-ldbi -ldl $(LIBCRYPT) 					   \
 		$(LIBOSMOGSM_LIBS) $(LIBOSMOVTY_LIBS) $(LIBOSMOCORE_LIBS)  \
-		$(LIBOSMOABIS_LIBS)
+		$(LIBOSMOABIS_LIBS) $(LIBSMPP34_LIBS)
diff --git a/openbsc/src/osmo-nitb/bsc_hack.c b/openbsc/src/osmo-nitb/bsc_hack.c
index 7427ead..fa38f1f 100644
--- a/openbsc/src/osmo-nitb/bsc_hack.c
+++ b/openbsc/src/osmo-nitb/bsc_hack.c
@@ -313,6 +313,9 @@
 	if (sms_queue_start(bsc_gsmnet, 20) != 0)
 		return -1;
 
+	if (smpp_openbsc_init(bsc_gsmnet, 6040) < 0)
+		return -1;
+
 	if (daemonize) {
 		rc = osmo_daemonize();
 		if (rc < 0) {