Replaced LAPD implementation with new LAPD core of libosmogsm

The core functionality of input/lapd.c is removed. Now it uses lapd_core.c
of libosmogsm. The stucture of lapd_instance and SAP (SAPI/TEI handling)
is kept, as well as interfaceing and header transcoding. The former
implementation of datalink is completely replaced, except for the minimal
TEI manager..

Currently it supports 3 modes:
- ABIS (BS11/Ericsson/Nokia)
- ISDN (Not used yet)
- ASAT (A interface for satellite)
all these modes can run as NETWORK or USER.

6 files changed, 482 insertions, 559 deletions

diff --git a/include/osmocom/abis/e1_input.h b/include/osmocom/abis/e1_input.h
index 0b946b2..51345f3 100644
--- a/include/osmocom/abis/e1_input.h
+++ b/include/osmocom/abis/e1_input.h
@@ -9,6 +9,7 @@
 #include <osmocom/core/msgb.h>
 #include <osmocom/core/select.h>
 #include <osmocom/abis/subchan_demux.h>
+#include <osmocom/abis/lapd.h>
 
 #define NUM_E1_TS   32
 
@@ -244,6 +245,10 @@ struct msgb *e1inp_tx_ts(struct e1inp_ts *e1i_ts,
 /* called by driver in case some kind of link state event */
 int e1inp_event(struct e1inp_ts *ts, int evt, uint8_t tei, uint8_t sapi);
 
+/* L2->L3 */
+void e1inp_dlsap_up(struct osmo_dlsap_prim *odp, uint8_t tei, uint8_t sapi,
+        void *rx_cbdata);
+
 /* Write LAPD frames to the fd. */
 void e1_set_pcap_fd(int fd);
 
diff --git a/include/osmocom/abis/lapd.h b/include/osmocom/abis/lapd.h
index 92dc2c3..ee4ee38 100644
--- a/include/osmocom/abis/lapd.h
+++ b/include/osmocom/abis/lapd.h
@@ -4,24 +4,26 @@
 #include <stdint.h>
 
 #include <osmocom/core/linuxlist.h>
+#include <osmocom/gsm/lapd_core.h>
 
-typedef enum {
-	LAPD_MPH_NONE	= 0,
-
-	LAPD_MPH_ACTIVATE_IND,
-	LAPD_MPH_DEACTIVATE_IND,
-
-	LAPD_DL_DATA_IND,
-	LAPD_DL_UNITDATA_IND,
-
-} lapd_mph_type;
+enum lapd_profile {
+	LAPD_PROFILE_ISDN,
+	LAPD_PROFILE_ABIS,
+	LAPD_PROFILE_ASAT,
+};
 
 struct lapd_instance {
 	struct llist_head list;		/* list of LAPD instances */
 	int network_side;
 
-	void (*transmit_cb)(uint8_t *data, int len, void *cbdata);
-	void *cbdata;
+	void (*transmit_cb)(struct msgb *msg, void *cbdata);
+	void *transmit_cbdata;
+	void (*receive_cb)(struct osmo_dlsap_prim *odp, uint8_t tei,
+		uint8_t sapi, void *rx_cbdata);
+	void *receive_cbdata;
+
+	enum lapd_profile profile;
+	uint8_t short_address;
 
 	struct llist_head tei_list;	/* list of TEI in this LAPD instance */
 };
@@ -34,19 +36,20 @@ enum lapd_recv_errors {
 	LAPD_ERR_UNKNOWN_U_CMD,
 	LAPD_ERR_UNKNOWN_TEI,
 	LAPD_ERR_BAD_CMD,
+	LAPD_ERR_NO_MEM,
 	__LAPD_ERR_MAX
 };
 
-extern uint8_t *lapd_receive(struct lapd_instance *li, uint8_t *data,
-			     unsigned int len, int *ilen, lapd_mph_type *prim,
-			     int *error);
+int lapd_receive(struct lapd_instance *li, struct msgb *msg, int *error);
 
-extern void lapd_transmit(struct lapd_instance *li, uint8_t tei, uint8_t sapi,
-			  uint8_t *data, unsigned int len);
+void lapd_transmit(struct lapd_instance *li, uint8_t tei, uint8_t sapi,
+		   struct msgb *msg);
 
 struct lapd_instance *lapd_instance_alloc(int network_side,
-					  void (*tx_cb)(uint8_t *data, int len,
-							void *cbdata), void *cbdata);
+	void (*tx_cb)(struct msgb *msg, void *cbdata), void *tx_cbdata,
+	void (*rx_cb)(struct osmo_dlsap_prim *odp, uint8_t tei, uint8_t sapi, 
+			void *rx_cbdata), void *rx_cbdata,
+	enum lapd_profile profile);
 
 void lapd_instance_free(struct lapd_instance *li);
 
diff --git a/src/e1_input.c b/src/e1_input.c
index cad39aa..1bd4744 100644
--- a/src/e1_input.c
+++ b/src/e1_input.c
@@ -505,11 +505,13 @@ int e1inp_rx_ts(struct e1inp_ts *ts, struct msgb *msg,
 		if (!link) {
 			LOGP(DLMI, LOGL_ERROR, "didn't find signalling link for "
 				"tei %d, sapi %d\n", tei, sapi);
+			msgb_free(msg);
 			return -EINVAL;
 		}
 		if (!ts->line->ops->sign_link) {
 	                LOGP(DLINP, LOGL_ERROR, "Fix your application, "
 				"no action set for signalling messages.\n");
+			msgb_free(msg);
 			return -ENOENT;
 		}
 		msg->dst = link;
@@ -517,10 +519,12 @@ int e1inp_rx_ts(struct e1inp_ts *ts, struct msgb *msg,
 		break;
 	case E1INP_TS_TYPE_TRAU:
 		ret = subch_demux_in(&ts->trau.demux, msg->l2h, msgb_l2len(msg));
+		msgb_free(msg);
 		break;
 	default:
 		ret = -EINVAL;
 		LOGP(DLMI, LOGL_ERROR, "unknown TS type %u\n", ts->type);
+		msgb_free(msg);
 		break;
 	}
 
@@ -538,18 +542,19 @@ int e1inp_rx_ts(struct e1inp_ts *ts, struct msgb *msg,
  */
 int e1inp_rx_ts_lapd(struct e1inp_ts *e1i_ts, struct msgb *msg)
 {
-	lapd_mph_type prim;
 	unsigned int sapi, tei;
-	int ilen, ret = 0, error = 0;
-	uint8_t *idata;
+	int ret = 0, error = 0;
 
 	sapi = msg->data[0] >> 2;
-	tei = msg->data[1] >> 1;
+	if ((msg->data[0] & 0x1))
+		tei = 0;
+	else
+		tei = msg->data[1] >> 1;
 
-	DEBUGP(DLMI, "<= len = %d, sapi(%d) tei(%d)", msg->len, sapi, tei);
+	DEBUGP(DLMI, "<= len = %d, sapi(%d) tei(%d)\n", msg->len, sapi, tei);
 
-	idata = lapd_receive(e1i_ts->lapd, msg->data, msg->len, &ilen, &prim, &error);
-	if (!idata) {
+	ret = lapd_receive(e1i_ts->lapd, msg, &error);
+	if (ret < 0) {
 		switch(error) {
 		case LAPD_ERR_UNKNOWN_TEI:
 			/* We don't know about this TEI, probably the BSC
@@ -560,39 +565,48 @@ int e1inp_rx_ts_lapd(struct e1inp_ts *e1i_ts, struct msgb *msg)
 			e1inp_event(e1i_ts, S_L_INP_TEI_UNKNOWN, tei, sapi);
 			return -EIO;
 		}
-		if (prim == 0)
-			return -EIO;
 	}
 
-	msgb_pull(msg, 2);
+	return 0;
+}
 
-	DEBUGP(DLMI, "prim %08x\n", prim);
+void e1inp_dlsap_up(struct osmo_dlsap_prim *dp, uint8_t tei, uint8_t sapi,
+        void *rx_cbdata)
+{
+	struct e1inp_ts *e1i_ts = rx_cbdata;
+	struct msgb *msg = dp->oph.msg;
 
-	switch (prim) {
-	case 0:
+	switch (dp->oph.primitive) {
+	case PRIM_DL_EST:
+		DEBUGP(DLMI, "DL_EST: sapi(%d) tei(%d)\n", sapi, tei);
+		e1inp_event(e1i_ts, S_L_INP_TEI_UP, tei, sapi);
 		break;
-	case LAPD_MPH_ACTIVATE_IND:
-		DEBUGP(DLMI, "MPH_ACTIVATE_IND: sapi(%d) tei(%d)\n", sapi, tei);
-		ret = e1inp_event(e1i_ts, S_L_INP_TEI_UP, tei, sapi);
+	case PRIM_DL_REL:
+		DEBUGP(DLMI, "DL_REL: sapi(%d) tei(%d)\n", sapi, tei);
+		e1inp_event(e1i_ts, S_L_INP_TEI_DN, tei, sapi);
 		break;
-	case LAPD_MPH_DEACTIVATE_IND:
-		DEBUGP(DLMI, "MPH_DEACTIVATE_IND: sapi(%d) tei(%d)\n", sapi, tei);
-		ret = e1inp_event(e1i_ts, S_L_INP_TEI_DN, tei, sapi);
+	case PRIM_DL_DATA:
+	case PRIM_DL_UNIT_DATA:
+		if (dp->oph.operation == PRIM_OP_INDICATION) {
+			msg->l2h = msg->l3h;
+			DEBUGP(DLMI, "RX: %s sapi=%d tei=%d\n",
+				osmo_hexdump(msgb_l2(msg), msgb_l2len(msg)),
+				sapi, tei);
+			e1inp_rx_ts(e1i_ts, msg, tei, sapi);
+			return;
+		}
 		break;
-	case LAPD_DL_DATA_IND:
-	case LAPD_DL_UNITDATA_IND:
-		if (prim == LAPD_DL_DATA_IND)
-			msg->l2h = msg->data + 2;
-		else
-			msg->l2h = msg->data + 1;
-		DEBUGP(DLMI, "RX: %s\n", osmo_hexdump(msgb_l2(msg), msgb_l2len(msg)));
-		ret = e1inp_rx_ts(e1i_ts, msg, tei, sapi);
+	case PRIM_MDL_ERROR:
+		DEBUGP(DLMI, "MDL_EERROR: cause(%d)\n", dp->u.error_ind.cause);
 		break;
 	default:
 		printf("ERROR: unknown prim\n");
 		break;
 	}
-	return ret;
+
+	msgb_free(msg);
+
+	return;
 }
 
 #define TSX_ALLOC_SIZE 4096
diff --git a/src/input/dahdi.c b/src/input/dahdi.c
index 4b39d6b..edc86ba 100644
--- a/src/input/dahdi.c
+++ b/src/input/dahdi.c
@@ -213,7 +213,7 @@ static void timeout_ts1_write(void *data)
 	ts_want_write(e1i_ts);
 }
 
-static void dahdi_write_msg(uint8_t *data, int len, void *cbdata)
+static void dahdi_write_msg(struct msgb *msg, void *cbdata)
 {
 	struct osmo_fd *bfd = cbdata;
 	struct e1inp_line *line = bfd->data;
@@ -221,7 +221,8 @@ static void dahdi_write_msg(uint8_t *data, int len, void *cbdata)
 	struct e1inp_ts *e1i_ts = &line->ts[ts_nr-1];
 	int ret;
 
-	ret = write(bfd->fd, data, len + 2);
+	ret = write(bfd->fd, msg->data, msg->len + 2);
+	msgb_free(msg);
 	if (ret == -1)
 		handle_dahdi_exception(e1i_ts);
 	else if (ret < 0)
@@ -247,8 +248,7 @@ static int handle_ts1_write(struct osmo_fd *bfd)
 
 	DEBUGP(DLMI, "TX: %s\n", osmo_hexdump(msg->data, msg->len));
 	lapd_transmit(e1i_ts->lapd, sign_link->tei,
-		      sign_link->sapi, msg->data, msg->len);
-	msgb_free(msg);
+			sign_link->sapi, msg);
 
 	/* set tx delay timer for next event */
 	e1i_ts->sign.tx_timer.cb = timeout_ts1_write;
@@ -354,7 +354,6 @@ static int handle_tsX_read(struct osmo_fd *bfd)
 	/* physical layer indicates that data has been sent,
 	 * we thus can send some more data */
 	ret = handle_tsX_write(bfd);
-	msgb_free(msg);
 
 	return ret;
 }
@@ -525,7 +524,9 @@ static int dahdi_e1_setup(struct e1inp_line *line)
 			bfd->when = BSC_FD_READ | BSC_FD_EXCEPT;
 			dahdi_set_bufinfo(bfd->fd, 1);
 			if (!e1i_ts->lapd)
-				e1i_ts->lapd = lapd_instance_alloc(1, dahdi_write_msg, bfd);
+				e1i_ts->lapd = lapd_instance_alloc(1,
+					dahdi_write_msg, bfd, e1inp_dlsap_up,
+					e1i_ts, LAPD_PROFILE_ABIS);
 			break;
 		case E1INP_TS_TYPE_TRAU:
 			/* close/release LAPD instance, if any */
diff --git a/src/input/lapd.c b/src/input/lapd.c
index be011af..d40b61c 100644
--- a/src/input/lapd.c
+++ b/src/input/lapd.c
@@ -4,6 +4,7 @@
  * (C) 2009 by Holger Hans Peter Freyther <zecke@selfish.org>
  * (C) 2010 by Digium and Matthew Fredrickson <creslin@digium.com>
  * (C) 2011 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2011 by Andreas Eversberg <jolly@eversberg.eu>
  *
  * All Rights Reserved
  *
@@ -23,27 +24,49 @@
  *
  */
 
-/* TODO:
-	* detect RR timeout and set SAP state back to SABM_RETRANSMIT
-	* use of value_string
-	* further code cleanup (spaghetti)
- */
-
 #include "internal.h"
 
 #include <stdio.h>
 #include <string.h>
 #include <errno.h>
 
-#include <osmocom/abis/lapd.h>
-
 #include <osmocom/core/linuxlist.h>
 #include <osmocom/core/logging.h>
 #include <osmocom/core/talloc.h>
 #include <osmocom/core/msgb.h>
 #include <osmocom/core/timer.h>
+#include <osmocom/abis/lapd.h>
+
+#define LAPD_ADDR2(sapi, cr) ((((sapi) & 0x3f) << 2) | (((cr) & 0x1) << 1))
+#define LAPD_ADDR3(tei) ((((tei) & 0x7f) << 1) | 0x1)
+
+#define LAPD_ADDR_SAPI(addr) ((addr) >> 2)
+#define LAPD_ADDR_CR(addr) (((addr) >> 1) & 0x1)
+#define LAPD_ADDR_EA(addr) ((addr) & 0x1)
+#define LAPD_ADDR_TEI(addr) ((addr) >> 1)
+
+#define LAPD_CTRL_I4(ns) (((ns) & 0x7f) << 1)
+#define LAPD_CTRL_I5(nr, p) ((((nr) & 0x7f) << 1) | ((p) & 0x1))
+#define LAPD_CTRL_S4(s) ((((s) & 0x3) << 2) | 0x1)
+#define LAPD_CTRL_S5(nr, p) ((((nr) & 0x7f) << 1) | ((p) & 0x1))
+#define LAPD_CTRL_U4(u, p) ((((u) & 0x1c) << (5-2)) | (((p) & 0x1) << 4) | (((u) & 0x3) << 2) | 0x3)
+
+#define LAPD_CTRL_is_I(ctrl)	(((ctrl) & 0x1) == 0)
+#define LAPD_CTRL_is_S(ctrl)	(((ctrl) & 0x3) == 1)
+#define LAPD_CTRL_is_U(ctrl)	(((ctrl) & 0x3) == 3)
+
+#define LAPD_CTRL_U_BITS(ctrl)	((((ctrl) & 0xC) >> 2) | ((ctrl) & 0xE0) >> 3)
+#define LAPD_CTRL_U_PF(ctrl)	(((ctrl) >> 4) & 0x1)
 
-#define SABM_INTERVAL		0, 300000
+#define LAPD_CTRL_S_BITS(ctrl)	(((ctrl) & 0xC) >> 2)
+#define LAPD_CTRL_S_PF(ctrl)	(ctrl & 0x1)
+
+#define LAPD_CTRL_I_Ns(ctrl)	(((ctrl) & 0xFE) >> 1)
+#define LAPD_CTRL_I_P(ctrl)	(ctrl & 0x1)
+#define LAPD_CTRL_Nr(ctrl)	(((ctrl) & 0xFE) >> 1)
+
+#define LAPD_LEN(len)	((len << 2) | 0x1)
+#define LAPD_EL	0x1
 
 typedef enum {
 	LAPD_TEI_NONE = 0,
@@ -57,65 +80,7 @@ const char *lapd_tei_states[] = {
 	"ACTIVE",
 };
 
-typedef enum {
-	LAPD_TYPE_NONE = 0,
-
-	LAPD_TYPE_I,
-	LAPD_TYPE_S,
-	LAPD_TYPE_U,
-} lapd_msg_type;
-
-typedef enum {
-	/* commands/responses */
-	LAPD_CMD_NONE = 0,
-
-	LAPD_CMD_I,
-	LAPD_CMD_RR,
-	LAPD_CMD_RNR,
-	LAPD_CMD_REJ,
-
-	LAPD_CMD_SABME,
-	LAPD_CMD_DM,
-	LAPD_CMD_UI,
-	LAPD_CMD_DISC,
-	LAPD_CMD_UA,
-	LAPD_CMD_FRMR,
-	LAPD_CMD_XID,
-} lapd_cmd_type;
-
-const char *lapd_cmd_types[] = {
-	"NONE",
-
-	"I",
-	"RR",
-	"RNR",
-	"REJ",
-
-	"SABME",
-	"DM",
-	"UI",
-	"DISC",
-	"UA",
-	"FRMR",
-	"XID",
-
-};
-
-enum lapd_sap_state {
-	SAP_STATE_INACTIVE,
-	SAP_STATE_SABM_RETRANS,
-	SAP_STATE_ACTIVE,
-};
-
-const char *lapd_sap_states[] = {
-	"INACTIVE",
-	"SABM_RETRANS",
-	"ACTIVE",
-};
-
-const char *lapd_msg_types = "?ISU";
-
-/* structure representing an allocated TEI within a LAPD instance */
+/* Structure representing an allocated TEI within a LAPD instance. */
 struct lapd_tei {
 	struct llist_head list;
 	struct lapd_instance *li;
@@ -125,58 +90,16 @@ struct lapd_tei {
 	struct llist_head sap_list;
 };
 
-/* Structure representing a SAP within a TEI. We use this for TE-mode to
- * re-transmit SABM */
+/* Structure representing a SAP within a TEI. It includes exactly one datalink
+ * instance. */
 struct lapd_sap {
 	struct llist_head list;
 	struct lapd_tei *tei;
 	uint8_t sapi;
-	enum lapd_sap_state state;
 
-	/* A valid N(R) value is one that is in the range V(A) ≤ N(R) ≤ V(S). */
-	int vs;			/* next to be transmitted */
-	int va;			/* last acked by peer */
-	int vr;			/* next expected to be received */
-
-	struct osmo_timer_list sabme_timer;	/* timer to re-transmit SABM message */
+	struct lapd_datalink dl;
 };
 
-/* 3.5.2.2   Send state variable V(S)
- * Each point-to-point data link connection endpoint shall have an associated V(S) when using I frame
- * commands. V(S) denotes the sequence number of the next I frame to be transmitted. The V(S) can
- * take on the value 0 through n minus 1. The value of V(S) shall be incremented by 1 with each
- * successive I frame transmission, and shall not exceed V(A) by more than the maximum number of
- * outstanding I frames k. The value of k may be in the range of 1 ≤ k ≤ 127.
- *
- * 3.5.2.3   Acknowledge state variable V(A)
- * Each point-to-point data link connection endpoint shall have an associated V(A) when using I frame
- * commands and supervisory frame commands/responses. V(A) identifies the last I frame that has been
- * acknowledged by its peer [V(A) − 1 equals the N(S) of the last acknowledged I frame]. V(A) can
- * take on the value 0 through n minus 1. The value of V(A) shall be updated by the valid N(R) values
- * received from its peer (see 3.5.2.6). A valid N(R) value is one that is in the range V(A) ≤ N(R) ≤
- * V(S).
- *
- * 3.5.2.5    Receive state variable V(R)
- * Each point-to-point data link connection endpoint shall have an associated V(R) when using I frame
- * commands and supervisory frame commands/responses. V(R) denotes the sequence number of the
- * next in-sequence I frame expected to be received. V(R) can take on the value 0 through n minus 1.
- * The value of V(R) shall be incremented by one with the receipt of an error-free, in-sequence I frame
- * whose N(S) equals V(R).
- */
-#define	LAPD_NS(sap) (sap->vs)
-#define	LAPD_NR(sap) (sap->vr)
-
-/* 3.5.2.4    Send sequence number N(S)
- * Only I frames contain N(S), the send sequence number of transmitted I frames. At the time that an in-
- * sequence I frame is designated for transmission, the value of N(S) is set equal to V(S).
- *
- * 3.5.2.6    Receive sequence number N(R)
- * All I frames and supervisory frames contain N(R), the expected send sequence number of the next
- * received I frame. At the time that a frame of the above types is designated for transmission, the value
- * of N(R) is set equal to V(R). N(R) indicates that the data link layer entity transmitting the N(R) has
- * correctly received all I frames numbered up to and including N(R) − 1.
- */
-
 /* Resolve TEI structure from given numeric TEI */
 static struct lapd_tei *teip_from_tei(struct lapd_instance *li, uint8_t tei)
 {
@@ -189,10 +112,12 @@ static struct lapd_tei *teip_from_tei(struct lapd_instance *li, uint8_t tei)
 	return NULL;
 };
 
+/* Change state of TEI */
 static void lapd_tei_set_state(struct lapd_tei *teip, int newstate)
 {
-	LOGP(DLMI, LOGL_INFO, "LAPD state change on TEI %d: %s -> %s\n", teip->tei,
-		   lapd_tei_states[teip->state], lapd_tei_states[newstate]);
+	LOGP(DLLAPD, LOGL_INFO, "LAPD state change on TEI %d: %s -> %s\n",
+		teip->tei, lapd_tei_states[teip->state],
+		lapd_tei_states[newstate]);
 	teip->state = newstate;
 };
 
@@ -228,52 +153,99 @@ static struct lapd_sap *lapd_sap_find(struct lapd_tei *teip, uint8_t sapi)
 	return NULL;
 }
 
-static void sabme_timer_cb(void *_sap);
+static int send_ph_data_req(struct lapd_msg_ctx *lctx, struct msgb *msg);
+static int send_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx);
 
 /* Allocate a new SAP within a given TEI */
 static struct lapd_sap *lapd_sap_alloc(struct lapd_tei *teip, uint8_t sapi)
 {
-	struct lapd_sap *sap = talloc_zero(teip, struct lapd_sap);
+	struct lapd_sap *sap;
+	struct lapd_datalink *dl;
+	struct lapd_instance *li;
+	int profile;
+	int n201, k;
+	int t200_sec, t200_usec;
+
+	sap = talloc_zero(teip, struct lapd_sap);
+	if (!sap)
+		return NULL;
 
-	LOGP(DLMI, LOGL_INFO, "LAPD Allocating SAP for SAPI=%u / TEI=%u\n",
+	LOGP(DLLAPD, LOGL_INFO, "LAPD Allocating SAP for SAPI=%u / TEI=%u\n",
 		sapi, teip->tei);
 
 	sap->sapi = sapi;
 	sap->tei = teip;
-	sap->sabme_timer.cb = &sabme_timer_cb;
-	sap->sabme_timer.data = sap;
+	dl = &sap->dl;
+	li = teip->li;
+
+	profile = li->profile;
+	switch (profile) {
+	case LAPD_PROFILE_ABIS:
+		n201 = 260;
+		t200_sec = 0; t200_usec = 240000;
+		if (sapi == 0)
+			k = 2;
+		else
+			k = 1;
+		break;
+	case LAPD_PROFILE_ASAT:
+		n201 = 260;
+		t200_sec = 2; t200_usec = 500000;
+		k = 15;
+		li->short_address = 1;
+		break;
+	case LAPD_PROFILE_ISDN:
+	default:
+		n201 = 260;
+		t200_sec = 1; t200_usec = 0;
+		k = 7;
+		break;
+	}
+
+	lapd_dl_init(dl, k, 128, n201);
+	dl->use_sabme = 1; /* use SABME instead of SABM (GSM) */
+	dl->send_ph_data_req = send_ph_data_req;
+	dl->send_dlsap = send_dlsap;
+	dl->lctx.dl = &sap->dl;
+	dl->lctx.sapi = sapi;
+	dl->lctx.tei = teip->tei;
+	dl->lctx.n201 = n201;
+	dl->t200_sec = t200_sec;
+	dl->t200_usec = t200_usec;
+
+	lapd_set_mode(&sap->dl, (teip->li->network_side) ? LAPD_MODE_NETWORK
+							: LAPD_MODE_USER);
 
 	llist_add(&sap->list, &teip->sap_list);
 
 	return sap;
 }
 
-static void lapd_sap_set_state(struct lapd_tei *teip, uint8_t sapi,
-				enum lapd_sap_state newstate)
+/* Free SAP instance, including the datalink */
+static void lapd_sap_free(struct lapd_sap *sap)
 {
-	struct lapd_sap *sap = lapd_sap_find(teip, sapi);
-	if (!sap)
-		return;
+	/* free datalink structures and timers */
+	lapd_dl_exit(&sap->dl);
 
-	LOGP(DLMI, LOGL_INFO, "LAPD state change on TEI %u / SAPI %u: "
-		"%s -> %s\n", teip->tei, sapi,
-		lapd_sap_states[sap->state], lapd_sap_states[newstate]);
-	switch (sap->state) {
-	case SAP_STATE_SABM_RETRANS:
-		if (newstate != SAP_STATE_SABM_RETRANS)
-			osmo_timer_del(&sap->sabme_timer);
-		break;
-	default:
-		if (newstate == SAP_STATE_SABM_RETRANS)
-			osmo_timer_schedule(&sap->sabme_timer, SABM_INTERVAL);
-		break;
+	llist_del(&sap->list);
+	talloc_free(sap);
+}
+
+/* Free TEI instance */
+static void lapd_tei_free(struct lapd_tei *teip)
+{
+	struct lapd_sap *sap, *sap2;
+
+	llist_for_each_entry_safe(sap, sap2, &teip->sap_list, list) {
+		lapd_sap_free(sap);
 	}
 
-	sap->state = newstate;
-};
+	llist_del(&teip->list);
+	talloc_free(teip);
+}
 
 /* Input function into TEI manager */
-static void lapd_tei_receive(struct lapd_instance *li, uint8_t *data, int len)
+static int lapd_tei_receive(struct lapd_instance *li, uint8_t *data, int len)
 {
 	uint8_t entity = data[0];
 	uint8_t ref = data[1];
@@ -282,355 +254,141 @@ static void lapd_tei_receive(struct lapd_instance *li, uint8_t *data, int len)
 	uint8_t e = data[4] & 1;
 	uint8_t resp[8];
 	struct lapd_tei *teip;
+	struct msgb *msg;
 
-	DEBUGP(DLMI, "LAPD TEIMGR: entity %x, ref %x, mt %x, action %x, e %x\n",
-		entity, ref, mt, action, e);
+	DEBUGP(DLLAPD, "LAPD TEIMGR: entity %x, ref %x, mt %x, action %x, "
+		"e %x\n", entity, ref, mt, action, e);
 
 	switch (mt) {
 	case 0x01:	/* IDENTITY REQUEST */
-		DEBUGP(DLMI, "LAPD TEIMGR: identity request for TEI %u\n", action);
+		DEBUGP(DLLAPD, "LAPD TEIMGR: identity request for TEI %u\n",
+			action);
 
 		teip = teip_from_tei(li, action);
 		if (!teip) {
-			LOGP(DLMI, LOGL_INFO, "TEI MGR: New TEI %u\n", action);
+			LOGP(DLLAPD, LOGL_INFO, "TEI MGR: New TEI %u\n",
+				action);
 			teip = lapd_tei_alloc(li, action);
+			if (!teip)
+				return -ENOMEM;
 		}
 
 		/* Send ACCEPT */
 		memmove(resp, "\xfe\xff\x03\x0f\x00\x00\x02\x00", 8);
 		resp[7] = (action << 1) | 1;
-		li->transmit_cb(resp, 8, li->cbdata);
+		msg = msgb_alloc_headroom(56, 56, "DL EST");
+		msg->l2h = msgb_push(msg, 8);
+		memcpy(msg->l2h, resp, 8);
+		LOGP(DLLAPD, LOGL_DEBUG, "TX: %s\n",
+			osmo_hexdump(msg->data, msg->len));
+		li->transmit_cb(msg, li->transmit_cbdata);
 
 		if (teip->state == LAPD_TEI_NONE)
 			lapd_tei_set_state(teip, LAPD_TEI_ASSIGNED);
 		break;
 	default:
-		LOGP(DLMI, LOGL_NOTICE, "LAPD TEIMGR: unknown mt %x action %x\n",
-		     mt, action);
+		LOGP(DLLAPD, LOGL_NOTICE, "LAPD TEIMGR: unknown mt %x "
+			"action %x\n", mt, action);
 		break;
 	};
-};
+
+	return 0;
+}
 
 /* General input function for any data received for this LAPD instance */
-uint8_t *
-lapd_receive(struct lapd_instance *li, uint8_t * data, unsigned int len,
-	     int *ilen, lapd_mph_type *prim, int *error)
+int lapd_receive(struct lapd_instance *li, struct msgb *msg, int *error)
 {
-	uint8_t sapi, cr, tei, command;
-	int pf, ns, nr;
-	uint8_t *contents;
-	struct lapd_tei *teip;
+	int i;
+	struct lapd_msg_ctx lctx;
+	int rc;
 	struct lapd_sap *sap;
+	struct lapd_tei *teip;
 
-	uint8_t resp[8];
-	int l = 0;
-
-	*ilen = 0;
-	*prim = 0;
-
-	if (len < 2) {
-		LOGP(DLMI, LOGL_ERROR, "LAPD receive len %d < 2, ignoring\n", len);
-		*error = LAPD_ERR_BAD_LEN;
-		return NULL;
-	};
-
-	if ((data[0] & 1) != 0 || (data[1] & 1) != 1) {
-		LOGP(DLMI, LOGL_ERROR, "LAPD address field %x/%x not well formed\n",
-			data[0], data[1]);
-		*error = LAPD_ERR_BAD_ADDR;
-		return NULL;
-	};
-
-	sapi = data[0] >> 2;
-	cr = (data[0] >> 1) & 1;
-	tei = data[1] >> 1;
-	command = li->network_side ^ cr;
-	//DEBUGP(DLMI, "  address sapi %x tei %d cmd %d cr %d\n", sapi, tei, command, cr);
-
-	if (len < 3) {
-		LOGP(DLMI, LOGL_ERROR, "LAPD receive len %d < 3, ignoring\n", len);
+	LOGP(DLLAPD, LOGL_DEBUG, "RX: %s\n", osmo_hexdump(msg->data, msg->len));
+	if (msg->len < 2) {
+		LOGP(DLLAPD, LOGL_ERROR, "LAPD receive len %d < 2, ignoring\n",
+			msg->len);
 		*error = LAPD_ERR_BAD_LEN;
-		return NULL;
-	};
-
-	lapd_msg_type typ = 0;
-	lapd_cmd_type cmd = 0;
-	pf = -1;
-	ns = -1;
-	nr = -1;
-	if ((data[2] & 1) == 0) {
-		typ = LAPD_TYPE_I;
-		if (len < 4) {
-			LOGP(DLMI, LOGL_ERROR, "LAPD I frame, len %d < 4\n", len);
-			*error = LAPD_ERR_BAD_LEN;
-			return NULL;
-		}
-		ns = data[2] >> 1;
-		nr = data[3] >> 1;
-		pf = data[3] & 1;
-		cmd = LAPD_CMD_I;
-	} else if ((data[2] & 3) == 1) {
-		typ = LAPD_TYPE_S;
-		if (len < 4) {
-			LOGP(DLMI, LOGL_ERROR, "LAPD S frame, len %d < 4\n", len);
-			*error = LAPD_ERR_BAD_LEN;
-			return NULL;
-		}
-		nr = data[3] >> 1;
-		pf = data[3] & 1;
-		switch (data[2]) {
-		case 0x1:
-			cmd = LAPD_CMD_RR;
-			break;
-		case 0x5:
-			cmd = LAPD_CMD_RNR;
-			break;
-		case 0x9:
-			cmd = LAPD_CMD_REJ;
-			break;
-		default:
-			LOGP(DLMI, LOGL_ERROR, "LAPD unknown S cmd %x\n", data[2]);
-			*error = LAPD_ERR_UNKNOWN_S_CMD;
-			return NULL;
-		};
-	} else if ((data[2] & 3) == 3) {
-		typ = LAPD_TYPE_U;
-		pf = (data[2] >> 4) & 1;
-		int val = data[2] & ~(1 << 4);
-		switch (val) {
-		case 0x6f:
-			cmd = LAPD_CMD_SABME;
-			break;
-		case 0x0f:
-			cmd = LAPD_CMD_DM;
-			break;
-		case 0x03:
-			cmd = LAPD_CMD_UI;
-			break;
-		case 0x43:
-			cmd = LAPD_CMD_DISC;
-			break;
-		case 0x63:
-			cmd = LAPD_CMD_UA;
-			break;
-		case 0x87:
-			cmd = LAPD_CMD_FRMR;
-			break;
-		case 0xaf:
-			cmd = LAPD_CMD_XID;
-			break;
-
-		default:
-			LOGP(DLMI, LOGL_ERROR, "LAPD unknown U cmd %x "
-			     "(pf %x data %x)\n", val, pf, data[2]);
-			*error = LAPD_ERR_UNKNOWN_U_CMD;
-			return NULL;
-		};
+		return -EINVAL;
 	};
+	msg->l2h = msg->data;
+
+	memset(&lctx, 0, sizeof(lctx));
+
+	i = 0;
+	/* adress field */
+	lctx.sapi = LAPD_ADDR_SAPI(msg->l2h[i]);
+	lctx.cr = LAPD_ADDR_CR(msg->l2h[i]);
+	lctx.lpd = 0;
+	if (!LAPD_ADDR_EA(msg->l2h[i])) {
+		i++;
+		lctx.tei = LAPD_ADDR_TEI(msg->l2h[i]);
+	}
+	i++;
+	/* control field */
+	if (LAPD_CTRL_is_I(msg->l2h[i])) {
+		lctx.format = LAPD_FORM_I;
+		lctx.n_send = LAPD_CTRL_I_Ns(msg->l2h[i]);
+		i++;
+		lctx.n_recv = LAPD_CTRL_Nr(msg->l2h[i]);
+		lctx.p_f = LAPD_CTRL_I_P(msg->l2h[i]);
+	} else if (LAPD_CTRL_is_S(msg->l2h[i])) {
+		lctx.format = LAPD_FORM_S;
+		lctx.s_u = LAPD_CTRL_S_BITS(msg->l2h[i]);
+		i++;
+		lctx.n_recv = LAPD_CTRL_Nr(msg->l2h[i]);
+		lctx.p_f = LAPD_CTRL_S_PF(msg->l2h[i]);
+	} else if (LAPD_CTRL_is_U(msg->l2h[i])) {
+		lctx.format = LAPD_FORM_U;
+		lctx.s_u = LAPD_CTRL_U_BITS(msg->l2h[i]);
+		lctx.p_f = LAPD_CTRL_U_PF(msg->l2h[i]);
+	} else
+		lctx.format = LAPD_FORM_UKN;
+	i++;
+	/* length */
+	msg->l3h = msg->l2h + i;
+	msgb_pull(msg, i);
+	lctx.length = msg->len;
+
+	/* perform TEI assignment, if received */
+	if (lctx.tei == 127) {
+		rc = lapd_tei_receive(li, msg->data, msg->len);
+		msgb_free(msg);
+		return rc;
+	}
 
-	contents = &data[4];
-	if (typ == LAPD_TYPE_U)
-		contents--;
-	*ilen = len - (contents - data);
-
-	if (tei == 127)
-		lapd_tei_receive(li, contents, *ilen);
-
-	teip = teip_from_tei(li, tei);
+	/* resolve TEI and SAPI */
+	teip = teip_from_tei(li, lctx.tei);
 	if (!teip) {
-		LOGP(DLMI, LOGL_NOTICE, "LAPD Unknown TEI %u\n", tei);
+		LOGP(DLLAPD, LOGL_NOTICE, "LAPD Unknown TEI %u\n", lctx.tei);
 		*error = LAPD_ERR_UNKNOWN_TEI;
-		return NULL;
+		msgb_free(msg);
+		return -EINVAL;
 	}
-
-	sap = lapd_sap_find(teip, sapi);
+	sap = lapd_sap_find(teip, lctx.sapi);
 	if (!sap) {
-		LOGP(DLMI, LOGL_INFO, "LAPD No SAP for TEI=%u / SAPI=%u, "
-			"allocating\n", tei, sapi);
-		sap = lapd_sap_alloc(teip, sapi);
-	}
-
-	DEBUGP(DLMI, "<- %c %s sapi %x tei %3d cmd %x pf %x ns %3d nr %3d "
-	     "ilen %d teip %p vs %d va %d vr %d len %d\n",
-	     lapd_msg_types[typ], lapd_cmd_types[cmd], sapi, tei, command, pf,
-	     ns, nr, *ilen, teip, sap->vs, sap->va, sap->vr, len);
-
-	switch (cmd) {
-	case LAPD_CMD_I:
-		if (ns != sap->vr) {
-			DEBUGP(DLMI, "LAPD ns %d != vr %d\n", ns, sap->vr);
-			if (ns == ((sap->vr - 1) & 0x7f)) {
-				LOGP(DLMI, LOGL_NOTICE, "LAPD double frame, "
-					"ignoring\n");
-				cmd = 0;	// ignore
-			} else {
-				LOGP(DLMI, LOGL_ERROR, "LAPD Out of order "
-					"ns %d != vr %d, ignoring\n", ns, sap->vr);
-				return NULL;
-			};
-		} else {
-			//printf("IN SEQUENCE\n");
-			sap->vr = (ns + 1) & 0x7f;	// FIXME: hack!
-		};
-
-		break;
-	case LAPD_CMD_UI:
-		break;
-	case LAPD_CMD_SABME:
-		sap->vs = 0;
-		sap->vr = 0;
-		sap->va = 0;
-
-		// ua
-		resp[l++] = data[0];
-		resp[l++] = (tei << 1) | 1;
-		resp[l++] = 0x73;
-		li->transmit_cb(resp, l, li->cbdata);
-		if (teip->state != LAPD_TEI_ACTIVE) {
-			if (teip->state == LAPD_TEI_ASSIGNED) {
-				lapd_tei_set_state(teip,
-						   LAPD_TEI_ACTIVE);
-				//printf("ASSIGNED and ACTIVE\n");
-			} else {
-#if 0
-				DEBUGP(DLMI, "rr in strange state, send rej\n");
-
-				// rej
-				resp[l++] = (sap-> sapi << 2) | (li->network_side ? 0 : 2);
-				resp[l++] = (tei << 1) | 1;
-				resp[l++] = 0x09;	//rej
-				resp[l++] = ((sap->vr + 1) << 1) | 0;
-				li->transmit_cb(resp, l, li->cbdata);
-				pf = 0;	// dont reply
-#endif
-			};
-		};
-
-		*prim = LAPD_MPH_ACTIVATE_IND;
-		break;
-	case LAPD_CMD_UA:
-		sap->vs = 0;
-		sap->vr = 0;
-		sap->va = 0;
-		lapd_tei_set_state(teip, LAPD_TEI_ACTIVE);
-		lapd_sap_set_state(teip, sapi, SAP_STATE_ACTIVE);
-		*prim = LAPD_MPH_ACTIVATE_IND;
-		break;
-	case LAPD_CMD_RR:
-		sap->va = (nr & 0x7f);
-#if 0
-		if (teip->state != LAPD_TEI_ACTIVE) {
-			if (teip->state == LAPD_TEI_ASSIGNED) {
-				lapd_tei_set_state(teip, LAPD_TEI_ACTIVE);
-				*prim = LAPD_MPH_ACTIVATE_IND;
-				//printf("ASSIGNED and ACTIVE\n");
-			} else {
-#if 0
-				DEBUGP(DLMI, "rr in strange " "state, send rej\n");
-
-				// rej
-				resp[l++] = (sap-> sapi << 2) | (li->network_side ? 0 : 2);
-				resp[l++] = (tei << 1) | 1;
-				resp[l++] = 0x09;	//rej
-				resp[l++] =
-				    ((sap->vr + 1) << 1) | 0;
-				li->transmit_cb(resp, l, li->cbdata);
-				pf = 0;	// dont reply
-#endif
-			};
-		};
-#endif
-		if (pf) {
-			// interrogating us, send rr
-			resp[l++] = data[0];
-			resp[l++] = (tei << 1) | 1;
-			resp[l++] = 0x01;	// rr
-			resp[l++] = (LAPD_NR(sap) << 1) | (data[3] & 1);	// pf bit from req
-
-			li->transmit_cb(resp, l, li->cbdata);
-
-		};
-		break;
-	case LAPD_CMD_FRMR:
-		// frame reject
-#if 0
-		if (teip->state == LAPD_TEI_ACTIVE)
-			*prim = LAPD_MPH_DEACTIVATE_IND;
-		lapd_tei_set_state(teip, LAPD_TEI_ASSIGNED);
-#endif
-		LOGP(DLMI, LOGL_NOTICE, "frame reject, ignoring\n");
-		break;
-	case LAPD_CMD_DISC:
-		// disconnect
-		resp[l++] = data[0];
-		resp[l++] = (tei << 1) | 1;
-		resp[l++] = 0x73;
-		li->transmit_cb(resp, l, li->cbdata);
-		lapd_tei_set_state(teip, LAPD_TEI_NONE);
-		break;
-	default:
-		LOGP(DLMI, LOGL_NOTICE, "unknown cmd for tei %d (cmd %x)\n",
-		     tei, cmd);
-		break;
-	}
-
-	if (typ == LAPD_TYPE_I) {
-		/* send rr
-		 * Thu Jan 22 19:17:13 2009 <4000> sangoma.c:340 read  (62/25)   4: fa 33 01 0a 
-		 * lapd <- S RR sapi 3e tei  25 cmd 0 pf 0 ns  -1 nr   5 ilen 0 teip 0x613800 vs 7 va 5 vr 2 len 4
-		 */
-
-		/* interrogating us, send rr */
-		DEBUGP(DLMI, "LAPD Sending RR response\n");
-		resp[l++] = data[0];
-		resp[l++] = (tei << 1) | 1;
-		resp[l++] = 0x01;	// rr
-		resp[l++] = (LAPD_NR(sap) << 1) | (data[3] & 1);	// pf bit from req
-
-		li->transmit_cb(resp, l, li->cbdata);
-
-		if (cmd != 0) {
-			*prim = LAPD_DL_DATA_IND;
-			return contents;
+		LOGP(DLLAPD, LOGL_INFO, "LAPD No SAP for TEI=%u / SAPI=%u, "
+			"allocating\n", lctx.tei, lctx.sapi);
+		sap = lapd_sap_alloc(teip, lctx.sapi);
+		if (!sap) {
+			*error = LAPD_ERR_NO_MEM;
+			msgb_free(msg);
+			return -ENOMEM;
 		}
-	} else if (tei != 127 && typ == LAPD_TYPE_U && cmd == LAPD_CMD_UI) {
-		*prim = LAPD_DL_UNITDATA_IND;
-		return contents;
 	}
+	lctx.dl = &sap->dl;
+	lctx.n201 = lctx.dl->maxf;
 
-	*error = LAPD_ERR_BAD_CMD;
-	return NULL;
-};
-
-/* low-level function to send a single SABM message */
-static int lapd_send_sabm(struct lapd_instance *li, uint8_t tei, uint8_t sapi)
-{
-	struct msgb *msg = msgb_alloc_headroom(1024, 128, "LAPD SABM");
-	if (!msg)
-		return -ENOMEM;
-
-	LOGP(DLMI, LOGL_INFO, "LAPD Sending SABM for TEI=%u, SAPI=%u\n", tei, sapi);
-
-	msgb_put_u8(msg, (sapi << 2) | (li->network_side ? 2 : 0));
-	msgb_put_u8(msg, (tei << 1) | 1);
-	msgb_put_u8(msg, 0x7F);
-
-	li->transmit_cb(msg->data, msg->len, li->cbdata);
-
-	msgb_free(msg);
-
-	return 0;
-}
-
-/* timer call-back function for SABM re-transmission */
-static void sabme_timer_cb(void *_sap)
-{
-	struct lapd_sap *sap = _sap;
-
-	lapd_send_sabm(sap->tei->li, sap->tei->tei, sap->sapi);
+	if (msg->len > lctx.n201) {
+		LOGP(DLLAPD, LOGL_ERROR, "message len %d > N201(%d) "
+			"(discarding)\n", msg->len, lctx.n201);
+		msgb_free(msg);
+		*error = LAPD_ERR_BAD_LEN;
+		return -EINVAL;
+	}
 
-	if (sap->state == SAP_STATE_SABM_RETRANS)
-		osmo_timer_schedule(&sap->sabme_timer, SABM_INTERVAL);
+	/* send to LAPD */
+	return lapd_ph_data_ind(msg, &lctx);
 }
 
 /* Start a (user-side) SAP for the specified TEI/SAPI on the LAPD instance */
@@ -638,6 +396,8 @@ int lapd_sap_start(struct lapd_instance *li, uint8_t tei, uint8_t sapi)
 {
 	struct lapd_sap *sap;
 	struct lapd_tei *teip;
+	struct osmo_dlsap_prim dp;
+	struct msgb *msg;
 
 	teip = teip_from_tei(li, tei);
 	if (!teip)
@@ -648,10 +408,19 @@ int lapd_sap_start(struct lapd_instance *li, uint8_t tei, uint8_t sapi)
 		return -EEXIST;
 
 	sap = lapd_sap_alloc(teip, sapi);
+	if (!sap)
+		return -ENOMEM;
 
-	lapd_sap_set_state(teip, sapi, SAP_STATE_SABM_RETRANS);
+	LOGP(DLLAPD, LOGL_NOTICE, "LAPD DL-ESTABLISH request TEI=%d SAPI=%d\n",
+		tei, sapi);
 
-	return 0;
+	/* prepare prim */
+	msg = msgb_alloc_headroom(56, 56, "DL EST");
+	msg->l3h = msg->data;
+        osmo_prim_init(&dp.oph, 0, PRIM_DL_EST, PRIM_OP_REQUEST, msg);
+
+        /* send to L2 */
+        return lapd_recv_dlsap(&dp, &sap->dl.lctx);
 }
 
 /* Stop a (user-side) SAP for the specified TEI/SAPI on the LAPD instance */
@@ -659,6 +428,8 @@ int lapd_sap_stop(struct lapd_instance *li, uint8_t tei, uint8_t sapi)
 {
 	struct lapd_tei *teip;
 	struct lapd_sap *sap;
+	struct osmo_dlsap_prim dp;
+	struct msgb *msg;
 
 	teip = teip_from_tei(li, tei);
 	if (!teip)
@@ -668,54 +439,138 @@ int lapd_sap_stop(struct lapd_instance *li, uint8_t tei, uint8_t sapi)
 	if (!sap)
 		return -ENODEV;
 
-	lapd_sap_set_state(teip, sapi, SAP_STATE_INACTIVE);
+	LOGP(DLLAPD, LOGL_NOTICE, "LAPD DL-RELEASE request TEI=%d SAPI=%d\n",
+		tei, sapi);
 
-	llist_del(&sap->list);
-	talloc_free(sap);
+	/* prepare prim */
+	msg = msgb_alloc_headroom(56, 56, "DL REL");
+	msg->l3h = msg->data;
+        osmo_prim_init(&dp.oph, 0, PRIM_DL_REL, PRIM_OP_REQUEST, msg);
 
-	return 0;
+        /* send to L2 */
+        return lapd_recv_dlsap(&dp, &sap->dl.lctx);
 }
 
-/* Transmit Data (I-Frame) on the given LAPD Instance / TEI / SAPI */
+/* Transmit Data (DL-DATA request) on the given LAPD Instance / TEI / SAPI */
 void lapd_transmit(struct lapd_instance *li, uint8_t tei, uint8_t sapi,
-		   uint8_t *data, unsigned int len)
+		   struct msgb *msg)
 {
 	struct lapd_tei *teip = teip_from_tei(li, tei);
 	struct lapd_sap *sap;
+	struct osmo_dlsap_prim dp;
 
 	if (!teip) {
-		LOGP(DLMI, LOGL_ERROR, "LAPD Cannot transmit on "
+		LOGP(DLLAPD, LOGL_ERROR, "LAPD Cannot transmit on "
 		     "non-existing TEI %u\n", tei);
+		msgb_free(msg);
 		return;
 	}
 
 	sap = lapd_sap_find(teip, sapi);
 	if (!sap) {
-		LOGP(DLMI, LOGL_INFO, "LAPD Tx on unknown SAPI=%u "
-		     "in TEI=%u, allocating\n", sapi, tei);
-		sap = lapd_sap_alloc(teip, sapi);
+		LOGP(DLLAPD, LOGL_INFO, "LAPD Tx on unknown SAPI=%u "
+		     "in TEI=%u\n", sapi, tei);
+		msgb_free(msg);
+		return;
+	}
+
+	/* prepare prim */
+	msg->l3h = msg->data;
+        osmo_prim_init(&dp.oph, 0, PRIM_DL_DATA, PRIM_OP_REQUEST, msg);
+
+        /* send to L2 */
+        lapd_recv_dlsap(&dp, &sap->dl.lctx);
+};
+
+static int send_ph_data_req(struct lapd_msg_ctx *lctx, struct msgb *msg)
+{
+	struct lapd_datalink *dl = lctx->dl;
+	struct lapd_sap *sap =
+		container_of(dl, struct lapd_sap, dl);
+	struct lapd_instance *li = sap->tei->li;
+	int format = lctx->format;
+	int addr_len;
+
+	/* control field */
+	switch (format) {
+	case LAPD_FORM_I:
+		msg->l2h = msgb_push(msg, 2);
+		msg->l2h[0] = LAPD_CTRL_I4(lctx->n_send);
+		msg->l2h[1] = LAPD_CTRL_I5(lctx->n_recv, lctx->p_f);
+		break;
+	case LAPD_FORM_S:
+		msg->l2h = msgb_push(msg, 2);
+		msg->l2h[0] = LAPD_CTRL_S4(lctx->s_u);
+		msg->l2h[1] = LAPD_CTRL_S5(lctx->n_recv, lctx->p_f);
+		break;
+	case LAPD_FORM_U:
+		msg->l2h = msgb_push(msg, 1);
+		msg->l2h[0] = LAPD_CTRL_U4(lctx->s_u, lctx->p_f);
+		break;
+	default:
+		msgb_free(msg);
+		return -EINVAL;
 	}
+	/* address field */
+	if (li->short_address && lctx->tei == 0)
+		addr_len = 1;
+	else
+		addr_len = 2;
+	msg->l2h = msgb_push(msg, addr_len);
+	msg->l2h[0] = LAPD_ADDR2(lctx->sapi, lctx->cr);
+	if (addr_len == 1)
+		msg->l2h[0] |= 0x1;
+	else
+		msg->l2h[1] = LAPD_ADDR3(lctx->tei);
+
+	/* forward frame to L1 */
+	LOGP(DLLAPD, LOGL_DEBUG, "TX: %s\n", osmo_hexdump(msg->data, msg->len));
+	li->transmit_cb(msg, li->transmit_cbdata);
 
-	/* prepend stuff */
-	uint8_t buf[10000];
-	memset(buf, 0, sizeof(buf));
-	memmove(buf + 4, data, len);
-	len += 4;
+	return 0;
+}
 
-	buf[0] = (sapi << 2) | (li->network_side ? 2 : 0);
-	buf[1] = (tei << 1) | 1;
-	buf[2] = (LAPD_NS(sap) << 1);
-	buf[3] = (LAPD_NR(sap) << 1) | 0;
+/* A DL-SAP message is received from datalink instance and forwarded to L3 */
+static int send_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx)
+{
+	struct lapd_datalink *dl = lctx->dl;
+	struct lapd_sap *sap =
+		container_of(dl, struct lapd_sap, dl);
+	struct lapd_instance *li;
+	uint8_t tei, sapi;
+	char *op = (dp->oph.operation == PRIM_OP_INDICATION) ? "indication" 
+							: "confirm";
+
+	li = sap->tei->li;
+	tei = lctx->tei;
+	sapi = lctx->sapi;
+
+	switch (dp->oph.primitive) {
+	case PRIM_DL_EST:
+		LOGP(DLLAPD, LOGL_NOTICE, "LAPD DL-ESTABLISH %s TEI=%d "
+			"SAPI=%d\n", op, lctx->tei, lctx->sapi);
+		break;
+	case PRIM_DL_REL:
+		LOGP(DLLAPD, LOGL_NOTICE, "LAPD DL-RELEASE %s TEI=%d "
+			"SAPI=%d\n", op, lctx->tei, lctx->sapi);
+		lapd_sap_free(sap);
+		/* note: sap and dl is now gone, don't use it anymore */
+		break;
+	default:
+		;
+	}
 
-	sap->vs = (sap->vs + 1) & 0x7f;
+	li->receive_cb(dp, tei, sapi, li->receive_cbdata);
 
-	li->transmit_cb(buf, len, li->cbdata);
-};
+	return 0;
+}
 
 /* Allocate a new LAPD instance */
 struct lapd_instance *lapd_instance_alloc(int network_side,
-					  void (*tx_cb)(uint8_t *data, int len,
-							void *cbdata), void *cbdata)
+	void (*tx_cb)(struct msgb *msg, void *cbdata), void *tx_cbdata,
+	void (*rx_cb)(struct osmo_dlsap_prim *odp, uint8_t tei, uint8_t sapi, 
+			void *rx_cbdata), void *rx_cbdata,
+	enum lapd_profile profile)
 {
 	struct lapd_instance *li;
 
@@ -723,9 +578,13 @@ struct lapd_instance *lapd_instance_alloc(int network_side,
 	if (!li)
 		return NULL;
 
-	li->transmit_cb = tx_cb;
-	li->cbdata = cbdata;
 	li->network_side = network_side;
+	li->transmit_cb = tx_cb;
+	li->transmit_cbdata = tx_cbdata;
+	li->receive_cb = rx_cb;
+	li->receive_cbdata = rx_cbdata;
+	li->profile = profile;
+
 	INIT_LLIST_HEAD(&li->tei_list);
 
 	return li;
@@ -733,18 +592,12 @@ struct lapd_instance *lapd_instance_alloc(int network_side,
 
 void lapd_instance_free(struct lapd_instance *li)
 {
-	struct lapd_tei *lt;
+	struct lapd_tei *teip, *teip2;
 
-	/* make sure to terminate any pending timers */
-	llist_for_each_entry(lt, &li->tei_list, list) {
-		struct lapd_sap *sap;
-		llist_for_each_entry(sap, &lt->sap_list, list) {
-			if (osmo_timer_pending(&sap->sabme_timer))
-				osmo_timer_del(&sap->sabme_timer);
-		}
+	/* Free all TEI instances */
+	llist_for_each_entry_safe(teip, teip2, &li->tei_list, list) {
+		lapd_tei_free(teip);
 	}
 
-	/* tei and sapis are allocated hierarchically of the lapd
-	 * instance, so one free is sufficient here */
 	talloc_free(li);
 }
diff --git a/src/input/misdn.c b/src/input/misdn.c
index fce6150..b5b57ae 100644
--- a/src/input/misdn.c
+++ b/src/input/misdn.c
@@ -67,6 +67,7 @@
 /*! \brief driver-specific data for \ref e1inp_line::driver_data */
 struct misdn_line {
 	int use_userspace_lapd;
+	int dummy_dchannel;
 };
 
 const struct value_string prim_names[] = {
@@ -109,11 +110,13 @@ static int handle_ts1_read(struct osmo_fd *bfd)
 		       (struct sockaddr *) &l2addr, &alen);
 	if (ret < 0) {
 		fprintf(stderr, "recvfrom error  %s\n", strerror(errno));
+		msgb_free(msg);
 		return ret;
 	}
 
 	if (alen != sizeof(l2addr)) {
 		fprintf(stderr, "%s error len\n", __func__);
+		msgb_free(msg);
 		return -EINVAL;
 	}
 
@@ -139,6 +142,7 @@ static int handle_ts1_read(struct osmo_fd *bfd)
 		}
 		/* save the channel number in the driver private struct */
 		link->driver.misdn.channel = l2addr.channel;
+		msgb_free(msg);
 		break;
 	case DL_ESTABLISH_IND:
 		DEBUGP(DLMI, "DL_ESTABLISH_IND: channel(%d) sapi(%d) tei(%d)\n",
@@ -155,11 +159,13 @@ static int handle_ts1_read(struct osmo_fd *bfd)
 		/* save the channel number in the driver private struct */
 		link->driver.misdn.channel = l2addr.channel;
 		ret = e1inp_event(e1i_ts, S_L_INP_TEI_UP, l2addr.tei, l2addr.sapi);
+		msgb_free(msg);
 		break;
 	case DL_RELEASE_IND:
 		DEBUGP(DLMI, "DL_RELEASE_IND: channel(%d) sapi(%d) tei(%d)\n",
 		l2addr.channel, l2addr.sapi, l2addr.tei);
 		ret = e1inp_event(e1i_ts, S_L_INP_TEI_DN, l2addr.tei, l2addr.sapi);
+		msgb_free(msg);
 		break;
 	case DL_DATA_IND:
 	case DL_UNITDATA_IND:
@@ -167,6 +173,7 @@ static int handle_ts1_read(struct osmo_fd *bfd)
 		DEBUGP(DLMI, "RX: %s\n", osmo_hexdump(msgb_l2(msg), ret - MISDN_HEADER_LEN));
 		if (mline->use_userspace_lapd) {
 			LOGP(DLMI, LOGL_ERROR, "DL_DATA_IND but userspace LAPD ?!?\n");
+			msgb_free(msg);
 			return -EIO;
 		}
 		ret = e1inp_rx_ts(e1i_ts, msg, l2addr.tei, l2addr.sapi);
@@ -174,10 +181,12 @@ static int handle_ts1_read(struct osmo_fd *bfd)
 	case PH_ACTIVATE_IND:
 		DEBUGP(DLMI, "PH_ACTIVATE_IND: channel(%d) sapi(%d) tei(%d)\n",
 		l2addr.channel, l2addr.sapi, l2addr.tei);
+		msgb_free(msg);
 		break;
 	case PH_DEACTIVATE_IND:
 		DEBUGP(DLMI, "PH_DEACTIVATE_IND: channel(%d) sapi(%d) tei(%d)\n",
 		l2addr.channel, l2addr.sapi, l2addr.tei);
+		msgb_free(msg);
 		break;
 	case PH_DATA_IND:
 		if (!mline->use_userspace_lapd) {
@@ -191,6 +200,7 @@ static int handle_ts1_read(struct osmo_fd *bfd)
 		ret = e1inp_rx_ts_lapd(e1i_ts, msg);
 		break;
 	default:
+		msgb_free(msg);
 		break;
 	}
 	return ret;
@@ -244,7 +254,7 @@ static int handle_ts1_write(struct osmo_fd *bfd)
 			line->num, sign_link->tei, sign_link->sapi,
 			osmo_hexdump(msg->data, msg->len));
 		lapd_transmit(e1i_ts->lapd, sign_link->tei,
-				sign_link->sapi, msg->data, msg->len);
+				sign_link->sapi, msg);
 	} else {
 		l2_data = msg->data;
 
@@ -266,9 +276,10 @@ static int handle_ts1_write(struct osmo_fd *bfd)
 			     (struct sockaddr *)&sa, sizeof(sa));
 		if (ret < 0)
 			fprintf(stderr, "%s sendto failed %d\n", __func__, ret);
+
+		msgb_free(msg);
 	}
 
-	msgb_free(msg);
 
 	/* set tx delay timer for next event */
 	e1i_ts->sign.tx_timer.cb = timeout_ts1_write;
@@ -279,20 +290,21 @@ static int handle_ts1_write(struct osmo_fd *bfd)
 }
 
 /*! \brief call-back from LAPD code, called when it wants to Tx data */
-static void misdn_write_msg(uint8_t *data, int len, void *cbdata)
+static void misdn_write_msg(struct msgb *msg, void *cbdata)
 {
 	struct osmo_fd *bfd = cbdata;
-	struct e1inp_line *line = bfd->data;
-	unsigned int ts_nr = bfd->priv_nr;
-	struct e1inp_ts *e1i_ts = &line->ts[ts_nr-1];
-	struct msgb *msg = msgb_alloc(1024, "mISDN PH_DATA_REQ");
+//	struct e1inp_line *line = bfd->data;
+//	unsigned int ts_nr = bfd->priv_nr;
+//	struct e1inp_ts *e1i_ts = &line->ts[ts_nr-1];
 	struct mISDNhead *hh;
 	int ret;
 
-	hh = (struct mISDNhead *) msgb_put(msg, MISDN_HEADER_LEN);
-	hh->prim = PH_DATA_REQ;
+	DEBUGP(DLMI, "PH_DATA_REQ: len=%d %s\n", msg->len,
+		osmo_hexdump(msg->data, msg->len));
 
-	memcpy(msgb_put(msg, len), data, len);
+	hh = (struct mISDNhead *) msgb_push(msg, MISDN_HEADER_LEN);
+	hh->prim = PH_DATA_REQ;
+	hh->id = 0;
 
 	ret = write(bfd->fd, msg->data, msg->len);
 	if (ret < 0)
@@ -315,6 +327,7 @@ static int handle_tsX_write(struct osmo_fd *bfd)
 
 	hh = (struct mISDNhead *) tx_buf;
 	hh->prim = PH_DATA_REQ;
+	hh->id = 0;
 
 	subchan_mux_out(mx, tx_buf+sizeof(*hh), BCHAN_TX_GRAN);
 
@@ -363,8 +376,7 @@ static int handle_tsX_read(struct osmo_fd *bfd)
 		msg->l2h = msg->data + MISDN_HEADER_LEN;
 		DEBUGP(DLMIB, "BCHAN RX: %s\n",
 			osmo_hexdump(msgb_l2(msg), ret - MISDN_HEADER_LEN));
-		ret = e1inp_rx_ts(e1i_ts, msg, 0, 0);
-		break;
+		return e1inp_rx_ts(e1i_ts, msg, 0, 0);
 	case PH_ACTIVATE_IND:
 	case PH_DATA_CNF:
 		/* physical layer indicates that data has been sent,
@@ -456,6 +468,37 @@ static int mi_e1_setup(struct e1inp_line *line, int release_l2)
 	struct misdn_line *mline = line->driver_data;
 	int ts, ret;
 
+	mline->dummy_dchannel = -1;
+	if (mline->use_userspace_lapd) {
+		/* Open dummy d-channel in order to use b-channels.
+		 * Also it is required to define the mode.
+		 */
+		if (mline->dummy_dchannel < 0) {
+			struct sockaddr_mISDN addr;
+
+			mline->dummy_dchannel = socket(PF_ISDN, SOCK_DGRAM,
+							ISDN_P_NT_E1);
+			if (mline->dummy_dchannel < 0) {
+				fprintf(stderr, "%s could not open socket %s\n",
+					__func__, strerror(errno));
+				return mline->dummy_dchannel;
+			}
+			memset(&addr, 0, sizeof(addr));
+			addr.family = AF_ISDN;
+			addr.dev = line->num;
+			addr.channel = 0;
+			addr.sapi = 0;
+			addr.tei = GROUP_TEI;
+			ret = bind(mline->dummy_dchannel,
+				(struct sockaddr *) &addr, sizeof(addr));
+			if (ret < 0) {
+				fprintf(stderr, "could not bind l2 socket %s\n",
+					strerror(errno));
+				return -EIO;
+			}
+		}
+	}
+
 	/* TS0 is CRC4, don't need any fd for it */
 	for (ts = 1; ts < NUM_E1_TS; ts++) {
 		unsigned int idx = ts-1;
@@ -473,9 +516,11 @@ static int mi_e1_setup(struct e1inp_line *line, int release_l2)
 			break;
 		case E1INP_TS_TYPE_SIGN:
 			if (mline->use_userspace_lapd)
-				bfd->fd = socket(PF_ISDN, SOCK_DGRAM, ISDN_P_B_HDLC);
+				bfd->fd = socket(PF_ISDN, SOCK_DGRAM,
+					ISDN_P_B_HDLC);
 			else
-				bfd->fd = socket(PF_ISDN, SOCK_DGRAM, ISDN_P_LAPD_NT);
+				bfd->fd = socket(PF_ISDN, SOCK_DGRAM,
+					ISDN_P_LAPD_NT);
 			bfd->when = BSC_FD_READ;
 			break;
 		case E1INP_TS_TYPE_TRAU:
@@ -500,7 +545,9 @@ static int mi_e1_setup(struct e1inp_line *line, int release_l2)
 		case E1INP_TS_TYPE_SIGN:
 			if (mline->use_userspace_lapd) {
 				addr.channel = ts;
-				e1i_ts->lapd = lapd_instance_alloc(1, misdn_write_msg, bfd);
+				e1i_ts->lapd = lapd_instance_alloc(1,
+					misdn_write_msg, bfd, e1inp_dlsap_up,
+					e1i_ts, LAPD_PROFILE_ABIS);
 			} else {
 				addr.channel = 0;
 				/* SAPI not supported yet in kernel */