Split of LAPDm into a core part and a GSM specific part

Instead of mixing together the GSM layer 1 interface and RSL interface
with the implementation of LAPD, the core function of LAPD is now
extracted from LAPDm. The core implementation is now in lapd_core.c
and lapd_core.h respectively.

The lapd_core.c implements exactly one datalink instance for one SAP.
The surrounding implementation "lapdm.c" codes/decodes the layer 2
headers and handles multiplexing and datalink instances, as well as
translates primitives from/to RSL layer.

lapd_core.c can now be used for other LAPD implementations. (ISDN/ABIS)

3 files changed, 2456 insertions, 1621 deletions

diff --git a/src/gsm/Makefile.am b/src/gsm/Makefile.am
index f5e46769..c0f72b3f 100644
--- a/src/gsm/Makefile.am
+++ b/src/gsm/Makefile.am
@@ -10,6 +10,6 @@ lib_LTLIBRARIES = libosmogsm.la
 libosmogsm_la_SOURCES = a5.c rxlev_stat.c tlv_parser.c comp128.c gsm_utils.c \
                         rsl.c gsm48.c gsm48_ie.c gsm0808.c sysinfo.c \
 			gprs_cipher_core.c gsm0480.c abis_nm.c gsm0502.c \
-			lapdm.c
+			lapd_core.c lapdm.c
 libosmogsm_la_LDFLAGS = -version-info $(LIBVERSION)
 libosmogsm_la_LIBADD = $(top_builddir)/src/libosmocore.la
diff --git a/src/gsm/lapd_core.c b/src/gsm/lapd_core.c
new file mode 100644
index 00000000..f7408e54
--- /dev/null
+++ b/src/gsm/lapd_core.c
@@ -0,0 +1,2128 @@
+/* LAPD core implementation */
+
+/* (C) 2010-2011 by Harald Welte <laforge@gnumonks.org>
+ * (C) 2010-2011 by Andreas Eversberg <jolly@eversberg.eu>
+ *
+ * All Rights Reserved
+ *
+ * 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.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ *
+ */
+
+/*! \addtogroup lapd
+ *  @{
+ */
+
+/*! \file lapd.c */
+
+/*!
+ * Notes on Buffering: rcv_buffer, tx_queue, tx_hist, send_buffer, send_queue
+ *
+ * RX data is stored in the rcv_buffer (pointer). If the message is complete, it
+ * is removed from rcv_buffer pointer and forwarded to L3. If the RX data is
+ * received while there is an incomplete rcv_buffer, it is appended to it.
+ *
+ * TX data is stored in the send_queue first. When transmitting a frame,
+ * the first message in the send_queue is moved to the send_buffer. There it
+ * resides until all fragments are acknowledged. Fragments to be sent by I
+ * frames are stored in the tx_hist buffer for resend, if required. Also the
+ * current fragment is copied into the tx_queue. There it resides until it is
+ * forwarded to layer 1.
+ *
+ * In case we have SAPI 0, we only have a window size of 1, so the unack-
+ * nowledged message resides always in the send_buffer. In case of a suspend,
+ * it can be written back to the first position of the send_queue.
+ *
+ * The layer 1 normally sends a PH-READY-TO-SEND. But because we use
+ * asynchronous transfer between layer 1 and layer 2 (serial link), we must
+ * send a frame before layer 1 reaches the right timeslot to send it. So we
+ * move the tx_queue to layer 1 when there is not already a pending frame, and
+ * wait until acknowledge after the frame has been sent. If we receive an
+ * acknowledge, we can send the next frame from the buffer, if any.
+ *
+ * The moving of tx_queue to layer 1 may also trigger T200, if desired. Also it
+ * will trigger next I frame, if possible.
+ *
+ * T203 is optional. It will be stated when entering MF EST state. It will also
+ * be started when I or S frame is received in that state . It will be
+ * restarted in the lapd_acknowledge() function, in case outstanding frames
+ * will not trigger T200. It will be stoped, when T200 is started in MF EST
+ * state. It will also be stoped when leaving MF EST state.
+ *
+ */
+
+/* Enable this to test content resolution on network side:
+ * - The first SABM is received, UA is dropped.
+ * - The phone repeats SABM, but it's content is wrong, so it is ignored
+ * - The phone repeats SABM again, content is right, so UA is sent.
+ */
+//#define TEST_CONTENT_RESOLUTION_NETWORK
+
+#include <stdio.h>
+#include <stdint.h>
+#include <string.h>
+#include <errno.h>
+#include <arpa/inet.h>
+
+#include <osmocom/core/logging.h>
+#include <osmocom/core/timer.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/utils.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/gsm/lapd_core.h>
+
+/* TS 04.06 Table 4 / Section 3.8.1 */
+#define LAPD_U_SABM	0x7
+#define LAPD_U_SABME	0xf
+#define LAPD_U_DM	0x3
+#define LAPD_U_UI	0x0
+#define LAPD_U_DISC	0x8
+#define LAPD_U_UA	0xC
+#define LAPD_U_FRMR	0x11
+
+#define LAPD_S_RR	0x0
+#define LAPD_S_RNR	0x1
+#define LAPD_S_REJ	0x2
+
+#define CR_USER2NET_CMD		0
+#define CR_USER2NET_RESP	1
+#define CR_NET2USER_CMD		1
+#define CR_NET2USER_RESP	0
+
+#define LAPD_HEADROOM	56
+
+#define SBIT(a) (1 << a)
+#define ALL_STATES 0xffffffff
+
+/* UTILITY FUNCTIONS */
+
+struct msgb *lapd_msgb_alloc(int length, const char *name)
+{
+	/* adding space for padding, FIXME: add as an option */
+	if (length < 21)
+		length = 21;
+	return msgb_alloc_headroom(length + LAPD_HEADROOM, LAPD_HEADROOM, name);
+}
+
+static inline uint8_t do_mod(uint8_t x, uint8_t m)
+{
+	return x & (m - 1);
+}
+
+static inline uint8_t inc_mod(uint8_t x, uint8_t m)
+{
+	return (x + 1) & (m - 1);
+}
+
+static inline uint8_t add_mod(uint8_t x, uint8_t y, uint8_t m)
+{
+	return (x + y) & (m - 1);
+}
+
+static inline uint8_t sub_mod(uint8_t x, uint8_t y, uint8_t m)
+{
+	return (x - y) & (m - 1); /* handle negative results correctly */
+}
+
+static void lapd_dl_flush_send(struct lapd_datalink *dl)
+{
+	struct msgb *msg;
+
+	/* Flush send-queue */
+	while ((msg = msgb_dequeue(&dl->send_queue)))
+		msgb_free(msg);
+
+	/* Clear send-buffer */
+	if (dl->send_buffer) {
+		msgb_free(dl->send_buffer);
+		dl->send_buffer = NULL;
+	}
+}
+
+static void lapd_dl_flush_hist(struct lapd_datalink *dl)
+{
+	unsigned int i;
+
+	for (i = 0; i < dl->range_hist; i++) {
+		if (dl->tx_hist[i].msg) {
+			msgb_free(dl->tx_hist[i].msg);
+			dl->tx_hist[i].msg = NULL;
+		}
+	}
+}
+
+static void lapd_dl_flush_tx(struct lapd_datalink *dl)
+{
+	struct msgb *msg;
+
+	while ((msg = msgb_dequeue(&dl->tx_queue)))
+		msgb_free(msg);
+	lapd_dl_flush_hist(dl);
+}
+
+/* Figure B.2/Q.921 */
+const char *lapd_state_names[] = {
+	"LAPD_STATE_NULL",
+	"LAPD_STATE_TEI_UNASS",
+	"LAPD_STATE_ASS_TEI_WAIT",
+	"LAPD_STATE_EST_TEI_WAIT",
+	"LAPD_STATE_IDLE",
+	"LAPD_STATE_SABM_SENT",
+	"LAPD_STATE_DISC_SENT",
+	"LAPD_STATE_MF_EST",
+	"LAPD_STATE_TIMER_RECOV",
+
+};
+
+static void lapd_dl_newstate(struct lapd_datalink *dl, uint32_t state)
+{
+	LOGP(DLLAPD, LOGL_INFO, "new state %s -> %s\n",
+		lapd_state_names[dl->state], lapd_state_names[state]);
+
+	if (state != LAPD_STATE_MF_EST && dl->state == LAPD_STATE_MF_EST) {
+		/* stop T203 on leaving MF EST state, if running */
+		if (osmo_timer_pending(&dl->t203)) {
+			LOGP(DLLAPD, LOGL_INFO, "stop T203\n");
+			osmo_timer_del(&dl->t203);
+		}
+		/* remove content res. (network side) on leaving MF EST state */
+		if (dl->cont_res) {
+			msgb_free(dl->cont_res);
+			dl->cont_res = NULL;
+		}
+	}
+
+	/* start T203 on entering MF EST state, if enabled */
+	if ((dl->t203_sec || dl->t203_usec)
+	 && state == LAPD_STATE_MF_EST && dl->state != LAPD_STATE_MF_EST) {
+		LOGP(DLLAPD, LOGL_INFO, "start T203\n");
+		osmo_timer_schedule(&dl->t203, dl->t203_sec, dl->t203_usec);
+	}
+
+	dl->state = state;
+}
+
+static void lapd_t200_cb(void *data);
+static void lapd_t203_cb(void *data);
+static int lapd_send_i(struct lapd_msg_ctx *lctx, int line);
+static int lapd_est_req(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx);
+
+static void *tall_lapd_ctx = NULL;
+
+/* init datalink instance and allocate history */
+void lapd_dl_init(struct lapd_datalink *dl, uint8_t k, uint8_t v_range,
+	int maxf)
+{
+	int m;
+
+	memset(dl, 0, sizeof(*dl));
+	INIT_LLIST_HEAD(&dl->send_queue);
+	INIT_LLIST_HEAD(&dl->tx_queue);
+	dl->reestablish = 1;
+	dl->n200_est_rel = 3;
+	dl->n200 = 3;
+	dl->t200_sec = 1;
+	dl->t200_usec = 0;
+	dl->t200.data = dl;
+	dl->t200.cb = &lapd_t200_cb;
+	dl->t203_sec = 10;
+	dl->t203_usec = 0;
+	dl->t203.data = dl;
+	dl->t203.cb = &lapd_t203_cb;
+	dl->maxf = maxf;
+	if (k > v_range - 1)
+		k = v_range - 1;
+	dl->k = k;
+	dl->v_range = v_range;
+
+	/* Calculate modulo for history array:
+	 * - The history range must be at least k+1.
+	 * - The history range must be 2^x, where x is as low as possible.
+	 */
+	k++;
+	for (m = 0x80; m; m >>= 1) {
+		if ((m & k)) {
+			if (k > m)
+				m <<= 1;
+			dl->range_hist = m;
+			break;
+		}
+	}
+
+	LOGP(DLLAPD, LOGL_INFO, "Init DL layer: sequence range = %d, k = %d, "
+		"history range = %d\n", dl->v_range, dl->k, dl->range_hist);
+
+	lapd_dl_newstate(dl, LAPD_STATE_IDLE);
+
+	if (!tall_lapd_ctx)
+		tall_lapd_ctx = talloc_named_const(NULL, 1, "lapd context");
+	dl->tx_hist = (struct lapd_history *) talloc_zero_array(tall_lapd_ctx,
+					struct log_info, dl->range_hist);
+}
+
+/* reset to IDLE state */
+void lapd_dl_reset(struct lapd_datalink *dl)
+{
+	if (dl->state == LAPD_STATE_IDLE)
+		return;
+	LOGP(DLLAPD, LOGL_INFO, "Resetting LAPDm instance\n");
+	/* enter idle state (and remove eventual cont_res) */
+	lapd_dl_newstate(dl, LAPD_STATE_IDLE);
+	/* flush buffer */
+	lapd_dl_flush_tx(dl);
+	lapd_dl_flush_send(dl);
+	/* Discard partly received L3 message */
+	if (dl->rcv_buffer) {
+		msgb_free(dl->rcv_buffer);
+		dl->rcv_buffer = NULL;
+	}
+	/* reset Timers */
+	osmo_timer_del(&dl->t200);
+	osmo_timer_del(&dl->t203);
+}
+
+/* reset and de-allocate history buffer */
+void lapd_dl_exit(struct lapd_datalink *dl)
+{
+	/* free all ressources except history buffer */
+	lapd_dl_reset(dl);
+	/* free history buffer list */
+	talloc_free(dl->tx_hist);
+}
+
+/*! \brief Set the \ref lapdm_mode of a LAPDm entity */
+int lapd_set_mode(struct lapd_datalink *dl, enum lapd_mode mode)
+{
+	switch (mode) {
+	case LAPD_MODE_USER:
+		dl->cr.loc2rem.cmd = CR_USER2NET_CMD;
+		dl->cr.loc2rem.resp = CR_USER2NET_RESP;
+		dl->cr.rem2loc.cmd = CR_NET2USER_CMD;
+		dl->cr.rem2loc.resp = CR_NET2USER_RESP;
+		break;
+	case LAPD_MODE_NETWORK:
+		dl->cr.loc2rem.cmd = CR_NET2USER_CMD;
+		dl->cr.loc2rem.resp = CR_NET2USER_RESP;
+		dl->cr.rem2loc.cmd = CR_USER2NET_CMD;
+		dl->cr.rem2loc.resp = CR_USER2NET_RESP;
+		break;
+	default:
+		return -EINVAL;
+	}
+	dl->mode = mode;
+
+	return 0;
+}
+
+/* send DL message with optional msgb */
+static int send_dl_l3(uint8_t prim, uint8_t op, struct lapd_msg_ctx *lctx,
+	struct msgb *msg)
+{
+	struct lapd_datalink *dl = lctx->dl;
+	struct osmo_dlsap_prim dp;
+
+	osmo_prim_init(&dp.oph, 0, prim, op, msg);
+	return dl->send_dlsap(&dp, lctx);
+}
+
+/* send simple DL message */
+static inline int send_dl_simple(uint8_t prim, uint8_t op,
+	struct lapd_msg_ctx *lctx)
+{
+	struct msgb *msg = lapd_msgb_alloc(0, "DUMMY");
+
+	return send_dl_l3(prim, op, lctx, msg);
+}
+
+/* send MDL-ERROR INDICATION */
+static int mdl_error(uint8_t cause, struct lapd_msg_ctx *lctx)
+{
+	struct lapd_datalink *dl = lctx->dl;
+	struct osmo_dlsap_prim dp;
+
+	LOGP(DLLAPD, LOGL_NOTICE, "sending MDL-ERROR-IND cause %d\n",
+		cause);
+	osmo_prim_init(&dp.oph, 0, PRIM_MDL_ERROR, PRIM_OP_INDICATION, NULL);
+	dp.u.error_ind.cause = cause;
+	return dl->send_dlsap(&dp, lctx);
+}
+
+/* send UA response */
+static int lapd_send_ua(struct lapd_msg_ctx *lctx, uint8_t len, uint8_t *data)
+{
+	struct msgb *msg = lapd_msgb_alloc(len, "LAPD UA");
+	struct lapd_msg_ctx nctx;
+	struct lapd_datalink *dl = lctx->dl;
+
+	memcpy(&nctx, lctx, sizeof(nctx));
+	msg->l3h = msgb_put(msg, len);
+	if (len)
+		memcpy(msg->l3h, data, len);
+	/* keep nctx.ldp */
+	/* keep nctx.sapi */
+	/* keep nctx.tei */
+	nctx.cr = dl->cr.loc2rem.resp;
+	nctx.format = LAPD_FORM_U;
+	nctx.s_u = LAPD_U_UA;
+	/* keep nctx.p_f */
+	nctx.length = len;
+	nctx.more = 0;
+
+	return dl->send_ph_data_req(&nctx, msg);
+}
+
+/* send DM response */
+static int lapd_send_dm(struct lapd_msg_ctx *lctx)
+{
+	struct msgb *msg = lapd_msgb_alloc(0, "LAPD DM");
+	struct lapd_msg_ctx nctx;
+	struct lapd_datalink *dl = lctx->dl;
+
+	memcpy(&nctx, lctx, sizeof(nctx));
+	/* keep nctx.ldp */
+	/* keep nctx.sapi */
+	/* keep nctx.tei */
+	nctx.cr = dl->cr.loc2rem.resp;
+	nctx.format = LAPD_FORM_U;
+	nctx.s_u = LAPD_U_DM;
+	/* keep nctx.p_f */
+	nctx.length = 0;
+	nctx.more = 0;
+
+	return dl->send_ph_data_req(&nctx, msg);
+}
+
+/* send RR response / command */
+static int lapd_send_rr(struct lapd_msg_ctx *lctx, uint8_t f_bit, uint8_t cmd)
+{
+	struct msgb *msg = lapd_msgb_alloc(0, "LAPD RR");
+	struct lapd_msg_ctx nctx;
+	struct lapd_datalink *dl = lctx->dl;
+
+	memcpy(&nctx, lctx, sizeof(nctx));
+	/* keep nctx.ldp */
+	/* keep nctx.sapi */
+	/* keep nctx.tei */
+	nctx.cr = (cmd) ? dl->cr.loc2rem.cmd : dl->cr.loc2rem.resp;
+	nctx.format = LAPD_FORM_S;
+	nctx.s_u = LAPD_S_RR;
+	nctx.p_f = f_bit;
+	nctx.n_recv = dl->v_recv;
+	nctx.length = 0;
+	nctx.more = 0;
+
+	return dl->send_ph_data_req(&nctx, msg);
+}
+
+/* send RNR response / command */
+static int lapd_send_rnr(struct lapd_msg_ctx *lctx, uint8_t f_bit, uint8_t cmd)
+{
+	struct msgb *msg = lapd_msgb_alloc(0, "LAPD RNR");
+	struct lapd_msg_ctx nctx;
+	struct lapd_datalink *dl = lctx->dl;
+
+	memcpy(&nctx, lctx, sizeof(nctx));
+	/* keep nctx.ldp */
+	/* keep nctx.sapi */
+	/* keep nctx.tei */
+	nctx.cr = (cmd) ? dl->cr.loc2rem.cmd : dl->cr.loc2rem.resp;
+	nctx.format = LAPD_FORM_S;
+	nctx.s_u = LAPD_S_RNR;
+	nctx.p_f = f_bit;
+	nctx.n_recv = dl->v_recv;
+	nctx.length = 0;
+	nctx.more = 0;
+
+	return dl->send_ph_data_req(&nctx, msg);
+}
+
+/* send REJ response */
+static int lapd_send_rej(struct lapd_msg_ctx *lctx, uint8_t f_bit)
+{
+	struct msgb *msg = lapd_msgb_alloc(0, "LAPD REJ");
+	struct lapd_msg_ctx nctx;
+	struct lapd_datalink *dl = lctx->dl;
+
+	memcpy(&nctx, lctx, sizeof(nctx));
+	/* keep nctx.ldp */
+	/* keep nctx.sapi */
+	/* keep nctx.tei */
+	nctx.cr = dl->cr.loc2rem.resp;
+	nctx.format = LAPD_FORM_S;
+	nctx.s_u = LAPD_S_REJ;
+	nctx.p_f = f_bit;
+	nctx.n_recv = dl->v_recv;
+	nctx.length = 0;
+	nctx.more = 0;
+
+	return dl->send_ph_data_req(&nctx, msg);
+}
+
+/* resend SABM or DISC message */
+static int lapd_send_resend(struct lapd_datalink *dl)
+{
+	struct msgb *msg;
+	uint8_t h = do_mod(dl->v_send, dl->range_hist);
+	int length = dl->tx_hist[h].msg->len;
+	struct lapd_msg_ctx nctx;
+
+	/* assemble message */
+	memcpy(&nctx, &dl->lctx, sizeof(nctx));
+	/* keep nctx.ldp */
+	/* keep nctx.sapi */
+	/* keep nctx.tei */
+	nctx.cr = dl->cr.loc2rem.cmd;
+	nctx.format = LAPD_FORM_U;
+	if (dl->state == LAPD_STATE_SABM_SENT)
+		nctx.s_u = (dl->use_sabme) ? LAPD_U_SABME : LAPD_U_SABM;
+	else
+		nctx.s_u = LAPD_U_DISC;
+	nctx.p_f = 1;
+	nctx.length = length;
+	nctx.more = 0;
+
+	/* Resend SABM/DISC from tx_hist */
+	msg = lapd_msgb_alloc(length, "LAPD resend");
+	msg->l3h = msgb_put(msg, length);
+	if (length)
+		memcpy(msg->l3h, dl->tx_hist[h].msg->data, length);
+
+	return dl->send_ph_data_req(&nctx, msg);
+}
+
+/* reestablish link */
+static int lapd_reestablish(struct lapd_datalink *dl)
+{
+	struct osmo_dlsap_prim dp;
+	struct msgb *msg;
+
+	msg = lapd_msgb_alloc(0, "DUMMY");
+	osmo_prim_init(&dp.oph, 0, PRIM_DL_EST, PRIM_OP_REQUEST, msg);
+	
+	return lapd_est_req(&dp, &dl->lctx);
+}
+
+/* Timer callback on T200 expiry */
+static void lapd_t200_cb(void *data)
+{
+	struct lapd_datalink *dl = data;
+
+	LOGP(DLLAPD, LOGL_INFO, "lapd_t200_cb(%p) state=%d\n", dl,
+		(int) dl->state);
+
+	switch (dl->state) {
+	case LAPD_STATE_SABM_SENT:
+		/* 5.4.1.3 */
+		if (dl->retrans_ctr + 1 >= dl->n200_est_rel + 1) {
+			/* send RELEASE INDICATION to L3 */
+			send_dl_simple(PRIM_DL_REL, PRIM_OP_INDICATION,
+				&dl->lctx);
+			/* send MDL ERROR INIDCATION to L3 */
+			mdl_error(MDL_CAUSE_T200_EXPIRED, &dl->lctx);
+			/* flush tx and send buffers */
+			lapd_dl_flush_tx(dl);
+			lapd_dl_flush_send(dl);
+			/* go back to idle state */
+			lapd_dl_newstate(dl, LAPD_STATE_IDLE);
+			/* NOTE: we must not change any other states or buffers
+			 * and queues, since we may reconnect after handover
+			 * failure. the buffered messages is replaced there */
+			break;
+		}
+		/* retransmit SABM command */
+		lapd_send_resend(dl);
+		/* increment re-transmission counter */
+		dl->retrans_ctr++;
+		/* restart T200 (PH-READY-TO-SEND) */
+		osmo_timer_schedule(&dl->t200, dl->t200_sec, dl->t200_usec);
+		break;
+	case LAPD_STATE_DISC_SENT:
+		/* 5.4.4.3 */
+		if (dl->retrans_ctr + 1 >= dl->n200_est_rel + 1) {
+			/* send RELEASE INDICATION to L3 */
+			send_dl_simple(PRIM_DL_REL, PRIM_OP_CONFIRM, &dl->lctx);
+			/* send MDL ERROR INIDCATION to L3 */
+			mdl_error(MDL_CAUSE_T200_EXPIRED, &dl->lctx);
+			/* flush tx and send buffers */
+			lapd_dl_flush_tx(dl);
+			lapd_dl_flush_send(dl);
+			/* go back to idle state */
+			lapd_dl_newstate(dl, LAPD_STATE_IDLE);
+			/* NOTE: we must not change any other states or buffers
+			 * and queues, since we may reconnect after handover
+			 * failure. the buffered messages is replaced there */
+			break;
+		}
+		/* retransmit DISC command */
+		lapd_send_resend(dl);
+		/* increment re-transmission counter */
+		dl->retrans_ctr++;
+		/* restart T200 (PH-READY-TO-SEND) */
+		osmo_timer_schedule(&dl->t200, dl->t200_sec, dl->t200_usec);
+		break;
+	case LAPD_STATE_MF_EST:
+		/* 5.5.7 */
+		dl->retrans_ctr = 0;
+		lapd_dl_newstate(dl, LAPD_STATE_TIMER_RECOV);
+		/* fall through */
+	case LAPD_STATE_TIMER_RECOV:
+		dl->retrans_ctr++;
+		if (dl->retrans_ctr < dl->n200) {
+			uint8_t vs = sub_mod(dl->v_send, 1, dl->v_range);
+			uint8_t h = do_mod(vs, dl->range_hist);
+			/* retransmit I frame (V_s-1) with P=1, if any */
+			if (dl->tx_hist[h].msg) {
+				struct msgb *msg;
+				int length = dl->tx_hist[h].msg->len;
+				struct lapd_msg_ctx nctx;
+
+				LOGP(DLLAPD, LOGL_INFO, "retransmit last frame"
+					" V(S)=%d\n", vs);
+				/* Create I frame (segment) from tx_hist */
+				memcpy(&nctx, &dl->lctx, sizeof(nctx));
+				/* keep nctx.ldp */
+				/* keep nctx.sapi */
+				/* keep nctx.tei */
+				nctx.cr = dl->cr.loc2rem.cmd;
+				nctx.format = LAPD_FORM_I;
+				nctx.p_f = 1;
+				nctx.n_send = vs;
+				nctx.n_recv = dl->v_recv;
+				nctx.length = length;
+				nctx.more = dl->tx_hist[h].more;
+				msg = lapd_msgb_alloc(length, "LAPD I resend");
+				msg->l3h = msgb_put(msg, length);
+				memcpy(msg->l3h, dl->tx_hist[h].msg->data,
+					length);
+				dl->send_ph_data_req(&nctx, msg);
+			} else {
+			/* OR send appropriate supervision frame with P=1 */
+				if (!dl->own_busy && !dl->seq_err_cond) {
+					lapd_send_rr(&dl->lctx, 1, 1);
+					/* NOTE: In case of sequence error
+					 * condition, the REJ frame has been
+					 * transmitted when entering the
+					 * condition, so it has not be done
+					 * here
+				 	 */
+				} else if (dl->own_busy) {
+					lapd_send_rnr(&dl->lctx, 1, 1);
+				} else {
+					LOGP(DLLAPD, LOGL_INFO, "unhandled, "
+						"pls. fix\n");
+				}
+			}
+			/* restart T200 (PH-READY-TO-SEND) */
+			osmo_timer_schedule(&dl->t200, dl->t200_sec,
+				dl->t200_usec);
+		} else {
+			/* send MDL ERROR INIDCATION to L3 */
+			mdl_error(MDL_CAUSE_T200_EXPIRED, &dl->lctx);
+			/* reestablish */
+			if (!dl->reestablish)
+				break;
+			LOGP(DLLAPD, LOGL_NOTICE, "N200 reached, performing "
+				"reestablishment.\n");
+			lapd_reestablish(dl);
+		}
+		break;
+	default:
+		LOGP(DLLAPD, LOGL_INFO, "T200 expired in unexpected "
+			"dl->state %d\n", (int) dl->state);
+	}
+}
+
+/* Timer callback on T203 expiry */
+static void lapd_t203_cb(void *data)
+{
+	struct lapd_datalink *dl = data;
+
+	LOGP(DLLAPD, LOGL_INFO, "lapd_t203_cb(%p) state=%d\n", dl,
+		(int) dl->state);
+
+	if (dl->state != LAPD_STATE_MF_EST) {
+		LOGP(DLLAPD, LOGL_ERROR, "T203 fired outside MF EST state, "
+			"please fix!\n");
+		return;
+	}
+
+	/* set retransmission counter to 0 */
+	dl->retrans_ctr = 0;
+	/* enter timer recovery state */
+	lapd_dl_newstate(dl, LAPD_STATE_TIMER_RECOV);
+	/* transmit a supervisory command with P bit set to 1 as follows: */
+	if (!dl->own_busy) {
+		LOGP(DLLAPD, LOGL_INFO, "transmit an RR poll command\n");
+		/* Send RR with P=1 */
+		lapd_send_rr(&dl->lctx, 1, 1);
+	} else {
+		LOGP(DLLAPD, LOGL_INFO, "transmit an RNR poll command\n");
+		/* Send RNR with P=1 */
+		lapd_send_rnr(&dl->lctx, 1, 1);
+	}
+	/* start T200 */
+	osmo_timer_schedule(&dl->t200, dl->t200_sec, dl->t200_usec);
+}
+
+/* 5.5.3.1: Common function to acknowlege frames up to the given N(R) value */
+static void lapd_acknowledge(struct lapd_msg_ctx *lctx)
+{
+	struct lapd_datalink *dl = lctx->dl;
+	uint8_t nr = lctx->n_recv;
+	int s = 0, rej = 0, t200_reset = 0, t200_start = 0;
+	int i, h;
+
+	/* supervisory frame ? */
+	if (lctx->format == LAPD_FORM_S)
+		s = 1;
+	/* REJ frame ? */
+	if (s && lctx->s_u == LAPD_S_REJ)
+	 	rej = 1;
+
+	/* Flush all transmit buffers of acknowledged frames */
+	for (i = dl->v_ack; i != nr; i = inc_mod(i, dl->v_range)) {
+		h = do_mod(i, dl->range_hist);
+		if (dl->tx_hist[h].msg) {
+			msgb_free(dl->tx_hist[h].msg);
+			dl->tx_hist[h].msg = NULL;
+			LOGP(DLLAPD, LOGL_INFO, "ack frame %d\n", i);
+		}
+	}
+
+	if (dl->state != LAPD_STATE_TIMER_RECOV) {
+		/* When not in the timer recovery condition, the data
+		 * link layer entity shall reset the timer T200 on
+		 * receipt of a valid I frame with N(R) higher than V(A),
+		 * or an REJ with an N(R) equal to V(A). */
+		if ((!rej && nr != dl->v_ack)
+		 || (rej && nr == dl->v_ack)) {
+			LOGP(DLLAPD, LOGL_INFO, "reset t200\n");
+		 	t200_reset = 1;
+			osmo_timer_del(&dl->t200);
+			/* 5.5.3.1 Note 1 + 2 imply timer recovery cond. */
+		}
+		/* 5.7.4: N(R) sequence error
+		 * N(R) is called valid, if and only if
+		 * (N(R)-V(A)) mod 8 <= (V(S)-V(A)) mod 8.
+		 */
+		if (sub_mod(nr, dl->v_ack, dl->v_range)
+				> sub_mod(dl->v_send, dl->v_ack, dl->v_range)) {
+			LOGP(DLLAPD, LOGL_NOTICE, "N(R) sequence error\n");
+			mdl_error(MDL_CAUSE_SEQ_ERR, lctx);
+		}
+	}
+
+	/* V(A) shall be set to the value of N(R) */
+	dl->v_ack = nr;
+
+	/* If T200 has been reset by the receipt of an I, RR or RNR frame,
+	 * and if there are outstanding I frames, restart T200 */
+	if (t200_reset && !rej) {
+		if (dl->tx_hist[sub_mod(dl->v_send, 1, dl->range_hist)].msg) {
+			LOGP(DLLAPD, LOGL_INFO, "start T200, due to unacked I "
+				"frame(s)\n");
+			t200_start = 1;
+			osmo_timer_schedule(&dl->t200, dl->t200_sec,
+				dl->t200_usec);
+		}
+	}
+
+	/* This also does a restart, when I or S frame is received */
+
+	/* Stop T203, if running */
+	if (osmo_timer_pending(&dl->t203)) {
+		osmo_timer_del(&dl->t203);
+		LOGP(DLLAPD, LOGL_INFO, "stop T203\n");
+	}
+	/* Start T203, if T200 is not running in MF EST state, if enabled */
+	if (!osmo_timer_pending(&dl->t200)
+	 && (dl->t203_sec || dl->t203_usec)
+	 && (dl->state == LAPD_STATE_MF_EST)) {
+		LOGP(DLLAPD, LOGL_INFO, "start T203\n");
+		osmo_timer_schedule(&dl->t203, dl->t203_sec, dl->t203_usec);
+	}
+}
+
+/* L1 -> L2 */
+
+/* Receive a LAPD U (Unnumbered) message from L1 */
+static int lapd_rx_u(struct msgb *msg, struct lapd_msg_ctx *lctx)
+{
+	struct lapd_datalink *dl = lctx->dl;
+	int length = lctx->length;
+	int rc;
+	uint8_t prim, op;
+
+	switch (lctx->s_u) {
+	case LAPD_U_SABM:
+	case LAPD_U_SABME:
+		prim = PRIM_DL_EST;
+		op = PRIM_OP_INDICATION;
+
+		LOGP(DLLAPD, LOGL_INFO, "SABM(E) received in state %s\n",
+			lapd_state_names[dl->state]);
+		/* 5.7.1 */
+		dl->seq_err_cond = 0;
+		/* G.2.2 Wrong value of the C/R bit */
+		if (lctx->cr == dl->cr.rem2loc.resp) {
+			LOGP(DLLAPD, LOGL_NOTICE, "SABM response error\n");
+			msgb_free(msg);
+			mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx);
+			return -EINVAL;
+		}
+
+		/* G.4.5 If SABM is received with L>N201 or with M bit
+		 * set, AN MDL-ERROR-INDICATION is sent to MM.
+		 */
+		if (lctx->more || length > lctx->n201) {
+			LOGP(DLLAPD, LOGL_NOTICE, "SABM too large error\n");
+			msgb_free(msg);
+			mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx);
+			return -EIO;
+		}
+
+		switch (dl->state) {
+		case LAPD_STATE_IDLE:
+			break;
+		case LAPD_STATE_MF_EST:
+			LOGP(DLLAPD, LOGL_INFO, "SABM command, multiple "
+				"frame established state\n");
+			/* If link is lost on the remote side, we start over
+			 * and send DL-ESTABLISH indication again. */
+			if (dl->v_send != dl->v_recv) {
+				LOGP(DLLAPD, LOGL_INFO, "Remote reestablish\n");
+				mdl_error(MDL_CAUSE_SABM_MF, lctx);
+				break;
+			}
+			/* Ignore SABM if content differs from first SABM. */
+			if (dl->mode == LAPD_MODE_NETWORK && length
+			 && dl->cont_res) {
+#ifdef TEST_CONTENT_RESOLUTION_NETWORK
+				dl->cont_res->data[0] ^= 0x01;
+#endif
+				if (memcmp(dl->cont_res, msg->data, length)) {
+					LOGP(DLLAPD, LOGL_INFO, "Another SABM "
+						"with diffrent content - "
+						"ignoring!\n");
+					msgb_free(msg);
+					return 0;
+				}
+			}
+			/* send UA again */
+			lapd_send_ua(lctx, length, msg->l3h);
+			msgb_free(msg);
+			return 0;
+		case LAPD_STATE_DISC_SENT:
+			/* 5.4.6.2 send DM with F=P */
+			lapd_send_dm(lctx);
+			/* reset Timer T200 */
+			osmo_timer_del(&dl->t200);
+			msgb_free(msg);
+			return send_dl_simple(prim, op, lctx);
+		default:
+			/* collision: Send UA, but still wait for rx UA, then
+			 * change to MF_EST state.
+			 */
+			/* check for contention resoultion */
+			if (dl->tx_hist[0].msg && dl->tx_hist[0].msg->len) {
+				LOGP(DLLAPD, LOGL_NOTICE, "SABM not allowed "
+					"during contention resolution\n");
+				mdl_error(MDL_CAUSE_SABM_INFO_NOTALL, lctx);
+			}
+			lapd_send_ua(lctx, length, msg->l3h);
+			msgb_free(msg);
+			return 0;
+		}
+		/* save message context for further use */
+		memcpy(&dl->lctx, lctx, sizeof(dl->lctx));
+#ifndef TEST_CONTENT_RESOLUTION_NETWORK
+		/* send UA response */
+		lapd_send_ua(lctx, length, msg->l3h);
+#endif
+		/* set Vs, Vr and Va to 0 */
+		dl->v_send = dl->v_recv = dl->v_ack = 0;
+		/* clear tx_hist */
+		lapd_dl_flush_hist(dl);
+		/* enter multiple-frame-established state */
+		lapd_dl_newstate(dl, LAPD_STATE_MF_EST);
+		/* store content resolution data on network side
+		 * Note: cont_res will be removed when changing state again,
+		 * so it must be allocated AFTER lapd_dl_newstate(). */
+		if (dl->mode == LAPD_MODE_NETWORK && length) {
+			dl->cont_res = lapd_msgb_alloc(length, "CONT RES");
+			memcpy(msgb_put(dl->cont_res, length), msg->l3h,
+				length);
+			LOGP(DLLAPD, LOGL_NOTICE, "Store content res.\n");
+		}
+		/* send notification to L3 */
+		if (length == 0) {
+			/* 5.4.1.2 Normal establishment procedures */
+			rc = send_dl_simple(prim, op, lctx);
+			msgb_free(msg);
+		} else {
+			/* 5.4.1.4 Contention resolution establishment */
+			rc = send_dl_l3(prim, op, lctx, msg);
+		}
+		break;
+	case LAPD_U_DM:
+		LOGP(DLLAPD, LOGL_INFO, "DM received in state %s\n",
+			lapd_state_names[dl->state]);
+		/* G.2.2 Wrong value of the C/R bit */
+		if (lctx->cr == dl->cr.rem2loc.cmd) {
+			LOGP(DLLAPD, LOGL_NOTICE, "DM command error\n");
+			msgb_free(msg);
+			mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx);
+			return -EINVAL;
+		}
+		if (!lctx->p_f) {
+			/* 5.4.1.2 DM responses with the F bit set to "0"
+			 * shall be ignored.
+			 */
+			msgb_free(msg);
+			return 0;
+		}
+		switch (dl->state) {
+		case LAPD_STATE_SABM_SENT:
+			break;
+		case LAPD_STATE_MF_EST:
+			if (lctx->p_f) {
+				LOGP(DLLAPD, LOGL_INFO, "unsolicited DM "
+					"response\n");
+				mdl_error(MDL_CAUSE_UNSOL_DM_RESP, lctx);
+			} else {
+				LOGP(DLLAPD, LOGL_INFO, "unsolicited DM "
+					"response, multiple frame established "
+					"state\n");
+				mdl_error(MDL_CAUSE_UNSOL_DM_RESP_MF, lctx);
+				/* reestablish */
+				if (!dl->reestablish) {
+					msgb_free(msg);
+					return 0;
+				}
+				LOGP(DLLAPD, LOGL_NOTICE, "Performing "
+					"reestablishment.\n");
+				lapd_reestablish(dl);
+			}
+			msgb_free(msg);
+			return 0;
+		case LAPD_STATE_TIMER_RECOV:
+			/* FP = 0 (DM is normal in case PF = 1) */
+			if (!lctx->p_f) {
+				LOGP(DLLAPD, LOGL_INFO, "unsolicited DM "
+					"response, multiple frame established "
+					"state\n");
+				mdl_error(MDL_CAUSE_UNSOL_DM_RESP_MF, lctx);
+				msgb_free(msg);
+				/* reestablish */
+				if (!dl->reestablish)
+					return 0;
+				LOGP(DLLAPD, LOGL_NOTICE, "Performing "
+					"reestablishment.\n");
+				return lapd_reestablish(dl);
+			}
+			break;
+		case LAPD_STATE_DISC_SENT:
+			/* reset Timer T200 */
+			osmo_timer_del(&dl->t200);
+			/* go to idle state */
+			lapd_dl_flush_tx(dl);
+			lapd_dl_flush_send(dl);
+			lapd_dl_newstate(dl, LAPD_STATE_IDLE);
+			rc = send_dl_simple(PRIM_DL_REL, PRIM_OP_CONFIRM, lctx);
+			msgb_free(msg);
+			return 0;
+		case LAPD_STATE_IDLE:
+			/* 5.4.5 all other frame types shall be discarded */
+		default:
+			LOGP(DLLAPD, LOGL_INFO, "unsolicited DM response! "
+				"(discarding)\n");
+			msgb_free(msg);
+			return 0;
+		}
+		/* reset T200 */
+		osmo_timer_del(&dl->t200);
+		/* go to idle state */
+		lapd_dl_newstate(dl, LAPD_STATE_IDLE);
+		rc = send_dl_simple(PRIM_DL_REL, PRIM_OP_INDICATION, lctx);
+		msgb_free(msg);
+		break;
+	case LAPD_U_UI:
+		LOGP(DLLAPD, LOGL_INFO, "UI received\n");
+		/* G.2.2 Wrong value of the C/R bit */
+		if (lctx->cr == dl->cr.rem2loc.resp) {
+			LOGP(DLLAPD, LOGL_NOTICE, "UI indicates response "
+				"error\n");
+			msgb_free(msg);
+			mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx);
+			return -EINVAL;
+		}
+
+		/* G.4.5 If UI is received with L>N201 or with M bit
+		 * set, AN MDL-ERROR-INDICATION is sent to MM.
+		 */
+		if (length > lctx->n201 || lctx->more) {
+			LOGP(DLLAPD, LOGL_NOTICE, "UI too large error "
+				"(%d > N201(%d) or M=%d)\n", length,
+				lctx->n201, lctx->more);
+			msgb_free(msg);
+			mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx);
+			return -EIO;
+		}
+
+		/* do some length checks */
+		if (length == 0) {
+			/* 5.3.3 UI frames received with the length indicator
+			 * set to "0" shall be ignored
+			 */
+			LOGP(DLLAPD, LOGL_INFO, "length=0 (discarding)\n");
+			msgb_free(msg);
+			return 0;
+		}
+		rc = send_dl_l3(PRIM_DL_UNIT_DATA, PRIM_OP_INDICATION, lctx,
+				msg);
+		break;
+	case LAPD_U_DISC:
+		prim = PRIM_DL_REL;
+		op = PRIM_OP_INDICATION;
+
+		LOGP(DLLAPD, LOGL_INFO, "DISC received in state %s\n",
+			lapd_state_names[dl->state]);
+		/* flush tx and send buffers */
+		lapd_dl_flush_tx(dl);
+		lapd_dl_flush_send(dl);
+		/* 5.7.1 */
+		dl->seq_err_cond = 0;
+		/* G.2.2 Wrong value of the C/R bit */
+		if (lctx->cr == dl->cr.rem2loc.resp) {
+			LOGP(DLLAPD, LOGL_NOTICE, "DISC response error\n");
+			msgb_free(msg);
+			mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx);
+			return -EINVAL;
+		}
+		if (length > 0 || lctx->more) {
+			/* G.4.4 If a DISC or DM frame is received with L>0 or
+			 * with the M bit set to "1", an MDL-ERROR-INDICATION
+			 * primitive with cause "U frame with incorrect
+			 * parameters" is sent to the mobile management entity.
+			 */
+			LOGP(DLLAPD, LOGL_NOTICE,
+				"U frame iwth incorrect parameters ");
+			msgb_free(msg);
+			mdl_error(MDL_CAUSE_UFRM_INC_PARAM, lctx);
+			return -EIO;
+		}
+		switch (dl->state) {
+		case LAPD_STATE_IDLE:
+			LOGP(DLLAPD, LOGL_INFO, "DISC in idle state\n");
+			/* send DM with F=P */
+			msgb_free(msg);
+			return lapd_send_dm(lctx);
+		case LAPD_STATE_SABM_SENT:
+			LOGP(DLLAPD, LOGL_INFO, "DISC in SABM state\n");
+			/* 5.4.6.2 send DM with F=P */
+			lapd_send_dm(lctx);
+			/* reset Timer T200 */
+			osmo_timer_del(&dl->t200);
+			/* go to idle state */
+			lapd_dl_newstate(dl, LAPD_STATE_IDLE);
+			msgb_free(msg);
+			return send_dl_simple(PRIM_DL_REL, PRIM_OP_INDICATION,
+				lctx);
+		case LAPD_STATE_MF_EST:
+		case LAPD_STATE_TIMER_RECOV:
+			LOGP(DLLAPD, LOGL_INFO, "DISC in est state\n");
+			break;
+		case LAPD_STATE_DISC_SENT:
+			LOGP(DLLAPD, LOGL_INFO, "DISC in disc state\n");
+			prim = PRIM_DL_REL;
+			op = PRIM_OP_CONFIRM;
+			break;
+		default:
+			lapd_send_ua(lctx, length, msg->l3h);
+			msgb_free(msg);
+			return 0;
+		}
+		/* send UA response */
+		lapd_send_ua(lctx, length, msg->l3h);
+		/* reset Timer T200 */
+		osmo_timer_del(&dl->t200);
+		/* enter idle state, keep tx-buffer with UA response */
+		lapd_dl_newstate(dl, LAPD_STATE_IDLE);
+		/* send notification to L3 */
+		rc = send_dl_simple(prim, op, lctx);
+		msgb_free(msg);
+		break;
+	case LAPD_U_UA:
+		LOGP(DLLAPD, LOGL_INFO, "UA received in state %s\n",
+			lapd_state_names[dl->state]);
+		/* G.2.2 Wrong value of the C/R bit */
+		if (lctx->cr == dl->cr.rem2loc.cmd) {
+			LOGP(DLLAPD, LOGL_NOTICE, "UA indicates command "
+				"error\n");
+			msgb_free(msg);
+			mdl_error(MDL_CAUSE_FRM_UNIMPL, lctx);
+			return -EINVAL;
+		}