Merge branch 'jolly/trx'

12 files changed, 278 insertions, 84 deletions

diff --git a/include/Makefile.am b/include/Makefile.am
index b94abec7..3875fecd 100644
--- a/include/Makefile.am
+++ b/include/Makefile.am
@@ -57,6 +57,7 @@ nobase_include_HEADERS = \
                        osmocom/gsm/meas_rep.h \
                        osmocom/gsm/mncc.h \
                        osmocom/gsm/prim.h \
+                       osmocom/gsm/l1sap.h \
                        osmocom/gsm/protocol/gsm_03_41.h \
                        osmocom/gsm/protocol/gsm_04_08.h \
                        osmocom/gsm/protocol/gsm_04_11.h \
diff --git a/include/osmocom/gprs/gprs_ns.h b/include/osmocom/gprs/gprs_ns.h
index 8fca70cb..8ca5a887 100644
--- a/include/osmocom/gprs/gprs_ns.h
+++ b/include/osmocom/gprs/gprs_ns.h
@@ -129,7 +129,10 @@ struct gprs_nsvc {
 /* Create a new NS protocol instance */
 struct gprs_ns_inst *gprs_ns_instantiate(gprs_ns_cb_t *cb, void *ctx);
 
-/* Destroy a NS protocol instance */
+/* Close a NS protocol instance */
+void gprs_ns_close(struct gprs_ns_inst *nsi);
+
+/* Close and Destroy a NS protocol instance */
 void gprs_ns_destroy(struct gprs_ns_inst *nsi);
 
 /* Listen for incoming GPRS packets via NS/UDP */
diff --git a/include/osmocom/gsm/l1sap.h b/include/osmocom/gsm/l1sap.h
new file mode 100644
index 00000000..c21abe5e
--- /dev/null
+++ b/include/osmocom/gsm/l1sap.h
@@ -0,0 +1,125 @@
+#ifndef _OSMOCOM_L1SAP_H
+#define _OSMOCOM_L1SAP_H
+
+#include <osmocom/core/prim.h>
+
+/*! \brief PH-SAP related primitives (L1<->L2 SAP) */
+enum osmo_ph_prim {
+	PRIM_PH_DATA,		/*!< \brief PH-DATA */
+	PRIM_PH_RACH,		/*!< \brief PH-RANDOM_ACCESS */
+	PRIM_PH_CONN,		/*!< \brief PH-CONNECT */
+	PRIM_PH_EMPTY_FRAME,	/*!< \brief PH-EMPTY_FRAME */
+	PRIM_PH_RTS,		/*!< \brief PH-RTS */
+	PRIM_MPH_INFO,		/*!< \brief MPH-INFO */
+	PRIM_TCH,		/*!< \brief TCH */
+	PRIM_TCH_RTS,		/*!< \brief TCH */
+};
+
+/*! \brief PH-SAP related primitives (L1<->L2 SAP) */
+enum osmo_mph_info_type {
+	PRIM_INFO_TIME,		/*!< \brief Current GSM time */
+	PRIM_INFO_MEAS,		/*!< \brief Measurement indication */
+	PRIM_INFO_ACTIVATE,	/*!< \brief Activation of channel */
+	PRIM_INFO_DEACTIVATE,	/*!< \brief Deactivation of channel */
+	PRIM_INFO_MODIFY,	/*!< \brief Mode Modify of channel */
+	PRIM_INFO_ACT_CIPH,	/*!< \brief Activation of ciphering */
+	PRIM_INFO_DEACT_CIPH,	/*!< \brief Deactivation of ciphering */
+};
+
+/*! \brief for PH-RANDOM_ACCESS.req */
+struct ph_rach_req_param {
+	uint8_t ra;		/*!< \brief Random Access */
+	uint8_t ta;		/*!< \brief Timing Advance */
+	uint8_t tx_power;	/*!< \brief Transmit Power */
+	uint8_t is_combined_ccch;/*!< \brief Are we using a combined CCCH? */
+	uint16_t offset;	/*!< \brief Timing Offset */
+};
+
+/*! \brief for PH-RANDOM_ACCESS.ind */
+struct ph_rach_ind_param {
+	uint8_t chan_nr;	/*!< \brief Channel Number (Like RSL) */
+	uint8_t ra;		/*!< \brief Random Access */
+	uint8_t acc_delay;	/*!< \brief Delay in bit periods */
+	uint32_t fn;		/*!< \brief GSM Frame Number at time of RA */
+};
+
+/*! \brief for PH-[UNIT]DATA.{req,ind} | PH-RTS.ind */
+struct ph_data_param {
+	uint8_t link_id;	/*!< \brief Link Identifier (Like RSL) */
+	uint8_t chan_nr;	/*!< \brief Channel Number (Like RSL) */
+	uint32_t fn;		/*!< \brief GSM Frame Number */
+	int8_t rssi;		/*!< \brief RSSI of receivedindication */
+};
+
+/*! \brief for TCH.{req,ind} | TCH-RTS.ind */
+struct ph_tch_param {
+	uint8_t chan_nr;	/*!< \brief Channel Number (Like RSL) */
+	uint32_t fn;		/*!< \brief GSM Frame Number */
+	int8_t rssi;		/*!< \brief RSSI of received indication */
+};
+
+/*! \brief for PH-CONN.ind */
+struct ph_conn_ind_param {
+	uint32_t fn;		/*!< \brief GSM Frame Number */
+};
+
+/*! \brief for TIME MPH-INFO.ind */
+struct info_time_ind_param {
+	uint32_t fn;		/*!< \brief GSM Frame Number */
+};
+
+/*! \brief for MEAS MPH-INFO.ind */
+struct info_meas_ind_param {
+	uint8_t chan_nr;	/*!< \brief Channel Number (Like RSL) */
+	uint16_t ber10k;	/*!< \brief BER in units of 0.01% */
+	int16_t ta_offs_qbits;	/*!< \brief timing advance offset (in qbits) */
+	int16_t c_i_cb;		/*!< \brief C/I ratio in 0.1 dB */
+	uint8_t is_sub:1;	/*!< \brief flags */
+	uint8_t inv_rssi;	/*!< \brief RSSI in dBm * -1 */
+};
+
+/*! \brief for {ACTIVATE,DEACTIVATE,MODIFY} MPH-INFO.req */
+struct info_act_req_param {
+	uint8_t chan_nr;	/*!< \brief Channel Number (Like RSL) */
+	uint8_t sacch_only;	/*!< \breif Only deactivate SACCH */
+};
+
+/*! \brief for {ACTIVATE,DEACTIVATE} MPH-INFO.cnf */
+struct info_act_cnf_param {
+	uint8_t chan_nr;	/*!< \brief Channel Number (Like RSL) */
+	uint8_t cause;		/*!< \brief RSL cause in case of nack */
+};
+
+/*! \brief for {ACTIVATE,DEACTIVATE} MPH-INFO.{req,cnf} */
+struct info_ciph_req_param {
+	uint8_t chan_nr;	/*!< \brief Channel Number (Like RSL) */
+	uint8_t downlink;	/*!< \brief Apply to downlink */
+	uint8_t uplink;		/*!< \brief Apply to uplink */
+};
+
+/*! \brief for MPH-INFO.ind */
+struct mph_info_param {
+	enum osmo_mph_info_type type; /*!< \brief Info message type */
+	union {
+		struct info_time_ind_param time_ind;
+		struct info_meas_ind_param meas_ind;
+		struct info_act_req_param act_req;
+		struct info_act_cnf_param act_cnf;
+		struct info_ciph_req_param ciph_req;
+	} u;
+};
+
+/*! \brief primitive header for PH-SAP primitives */
+struct osmo_phsap_prim {
+	struct osmo_prim_hdr oph; /*!< \brief generic primitive header */
+	union {
+		struct ph_data_param data;
+		struct ph_tch_param tch;
+		struct ph_rach_req_param rach_req;
+		struct ph_rach_ind_param rach_ind;
+		struct ph_conn_ind_param conn_ind;
+		struct mph_info_param info;
+	} u;			/*!< \brief request-specific data */
+};
+
+#endif /* _OSMOCOM_L1SAP_H */
diff --git a/include/osmocom/gsm/lapdm.h b/include/osmocom/gsm/lapdm.h
index 571fd460..a6e4ad72 100644
--- a/include/osmocom/gsm/lapdm.h
+++ b/include/osmocom/gsm/lapdm.h
@@ -1,6 +1,7 @@
 #ifndef _OSMOCOM_LAPDM_H
 #define _OSMOCOM_LAPDM_H
 
+#include <osmocom/gsm/l1sap.h>
 #include <osmocom/gsm/lapd_core.h>
 
 /*! \defgroup lapdm LAPDm implementation according to GSM TS 04.06
@@ -9,55 +10,6 @@
 
 /*! \file lapdm.h */
 
-/* primitive related sutff */
-
-/*! \brief LAPDm related primitives (L1<->L2 SAP) */
-enum osmo_ph_prim {
-	PRIM_PH_DATA,		/*!< \brief PH-DATA */
-	PRIM_PH_RACH,		/*!< \brief PH-RANDOM_ACCESS */
-	PRIM_PH_CONN,		/*!< \brief PH-CONNECT */
-	PRIM_PH_EMPTY_FRAME,	/*!< \brief PH-EMPTY_FRAME */
-	PRIM_PH_RTS,		/*!< \brief PH-RTS */
-};
-
-/*! \brief for PH-RANDOM_ACCESS.req */
-struct ph_rach_req_param {
-	uint8_t ra;		/*!< \brief Random Access */
-	uint8_t ta;		/*!< \brief Timing Advance */
-	uint8_t tx_power;	/*!< \brief Transmit Power */
-	uint8_t is_combined_ccch;/*!< \brief Are we using a combined CCCH? */
-	uint16_t offset;	/*!< \brief Timing Offset */
-};
-
-/*! \brief for PH-RANDOM_ACCESS.ind */
-struct ph_rach_ind_param {
-	uint8_t ra;		/*!< \brief Random Access */
-	uint8_t acc_delay;	/*!< \brief Delay in bit periods */
-	uint32_t fn;		/*!< \brief GSM Frame Number at time of RA */
-};
-
-/*! \brief for PH-[UNIT]DATA.{req,ind} */
-struct ph_data_param {
-	uint8_t link_id;	/*!< \brief Link Identifier (Like RSL) */
-	uint8_t chan_nr;	/*!< \brief Channel Number (Like RSL) */
-};
-
-/*! \brief for PH-CONN.ind */
-struct ph_conn_ind_param {
-	uint32_t fn;		/*!< \brief GSM Frame Number */
-};
-
-/*! \brief primitive header for LAPDm PH-SAP primitives */
-struct osmo_phsap_prim {
-	struct osmo_prim_hdr oph; /*!< \brief generic primitive header */
-	union {
-		struct ph_data_param data;
-		struct ph_rach_req_param rach_req;
-		struct ph_rach_ind_param rach_ind;
-		struct ph_conn_ind_param conn_ind;
-	} u;			/*!< \brief request-specific data */
-};
-
 /*! \brief LAPDm mode/role */
 enum lapdm_mode {
 	LAPDM_MODE_MS,		/*!< \brief behave like a MS (mobile phone) */
diff --git a/include/osmocom/gsm/protocol/gsm_04_08.h b/include/osmocom/gsm/protocol/gsm_04_08.h
index bb8a87ed..bd6d707a 100644
--- a/include/osmocom/gsm/protocol/gsm_04_08.h
+++ b/include/osmocom/gsm/protocol/gsm_04_08.h
@@ -1052,9 +1052,9 @@ enum gsm48_rr_cause {
 	GSM48_RR_CAUSE_ABNORMAL_TIMER	= 0x03,
 	GSM48_RR_CAUSE_ABNORMAL_NOACT	= 0x04,
 	GSM48_RR_CAUSE_PREMPTIVE_REL	= 0x05,
-	GSM48_RR_CAUSE_HNDOVER_IMP	= 0x06,
-	GSM48_RR_CAUSE_CHAN_MODE_UNACCT	= 0x07,
-	GSM48_RR_CAUSE_FREQ_NOT_IMPL	= 0x08,
+	GSM48_RR_CAUSE_HNDOVER_IMP	= 0x08,
+	GSM48_RR_CAUSE_CHAN_MODE_UNACCT	= 0x09,
+	GSM48_RR_CAUSE_FREQ_NOT_IMPL	= 0x0a,
 	GSM48_RR_CAUSE_CALL_CLEARED	= 0x41,
 	GSM48_RR_CAUSE_SEMANT_INCORR	= 0x5f,
 	GSM48_RR_CAUSE_INVALID_MAND_INF = 0x60,
diff --git a/src/gb/gprs_ns.c b/src/gb/gprs_ns.c
index ef937d9e..5620b3a7 100644
--- a/src/gb/gprs_ns.c
+++ b/src/gb/gprs_ns.c
@@ -917,13 +917,7 @@ struct gprs_ns_inst *gprs_ns_instantiate(gprs_ns_cb_t *cb, void *ctx)
 	return nsi;
 }
 
-/*! \brief Destroy an entire NS instance
- *  \param nsi gprs_ns_inst that is to be destroyed
- *
- *  This function releases all resources associated with the
- *  NS-instance.
- */
-void gprs_ns_destroy(struct gprs_ns_inst *nsi)
+void gprs_ns_close(struct gprs_ns_inst *nsi)
 {
 	struct gprs_nsvc *nsvc, *nsvc2;
 
@@ -935,8 +929,19 @@ void gprs_ns_destroy(struct gprs_ns_inst *nsi)
 	if (nsi->nsip.fd.data) {
 		close(nsi->nsip.fd.fd);
 		osmo_fd_unregister(&nsi->nsip.fd);
+		nsi->nsip.fd.data = NULL;
 	}
+}
 
+/*! \brief Destroy an entire NS instance
+ *  \param nsi gprs_ns_inst that is to be destroyed
+ *
+ *  This function releases all resources associated with the
+ *  NS-instance.
+ */
+void gprs_ns_destroy(struct gprs_ns_inst *nsi)
+{
+	gprs_ns_close(nsi);
 	/* free the NSI */
 	talloc_free(nsi);
 }
diff --git a/src/gb/libosmogb.map b/src/gb/libosmogb.map
index d65819b4..7af085c5 100644
--- a/src/gb/libosmogb.map
+++ b/src/gb/libosmogb.map
@@ -38,6 +38,7 @@ bssgp_nsi;
 
 gprs_ns_cause_str;
 gprs_ns_destroy;
+gprs_ns_close;
 gprs_ns_frgre_listen;
 gprs_ns_frgre_sendmsg;
 gprs_ns_instantiate;
diff --git a/src/gsm/gsm0480.c b/src/gsm/gsm0480.c
index b9b3ed97..89d43c84 100644
--- a/src/gsm/gsm0480.c
+++ b/src/gsm/gsm0480.c
@@ -85,7 +85,7 @@ struct msgb *gsm0480_create_unstructuredSS_Notify(int alertPattern, const char *
 {
 	struct msgb *msg;
 	uint8_t *seq_len_ptr, *ussd_len_ptr, *data;
-	int len;
+	int len, octet_len;
 
 	msg = msgb_alloc_headroom(1024, 128, "GSM 04.80");
 	if (!msg)
@@ -106,8 +106,13 @@ struct msgb *gsm0480_create_unstructuredSS_Notify(int alertPattern, const char *
 	ussd_len_ptr = msgb_put(msg, 1);
 	data = msgb_put(msg, 0);
 	len = gsm_7bit_encode(data, text);
-	msgb_put(msg, len);
-	ussd_len_ptr[0] = len;
+	octet_len = len*7/8;
+	if (len*7%8 != 0)
+		octet_len++;
+	/* Warning, len indicates the amount of septets
+	 * (characters), we need amount of octets occupied */
+	msgb_put(msg, octet_len);
+	ussd_len_ptr[0] = octet_len;
 	/* USSD-String } */
 
 	/* alertingPattern { */
@@ -127,7 +132,7 @@ struct msgb *gsm0480_create_notifySS(const char *text)
 	struct msgb *msg;
 	uint8_t *data, *tmp_len;
 	uint8_t *seq_len_ptr, *cal_len_ptr, *opt_len_ptr, *nam_len_ptr;
-	int len;
+	int len, octet_len;
 
 	len = strlen(text);
 	if (len < 1 || len > 160)
@@ -173,12 +178,17 @@ struct msgb *gsm0480_create_notifySS(const char *text)
 	tmp_len = msgb_put(msg, 1);
 	data = msgb_put(msg, 0);
 	len = gsm_7bit_encode(data, text);
-	tmp_len[0] = len;
-	msgb_put(msg, len);
+	octet_len = len*7/8;
+	if (len*7%8 != 0)
+		octet_len++;
+	/* Warning, len indicates the amount of septets
+	 * (characters), we need amount of octets occupied */
+	tmp_len[0] = octet_len;
+	msgb_put(msg, octet_len);
 
 	/* }; namePresentationAllowed */
 
-	cal_len_ptr[0] = 3 + 3 + 2 + len;
+	cal_len_ptr[0] = 3 + 3 + 2 + octet_len;
 	opt_len_ptr[0] = cal_len_ptr[0] + 2;
 	/* }; callingName */
 
@@ -415,7 +425,7 @@ struct msgb *gsm0480_create_ussd_resp(uint8_t invoke_id, uint8_t trans_id, const
 	struct msgb *msg;
 	struct gsm48_hdr *gh;
 	uint8_t *ptr8;
-	int response_len;
+	int response_len, octet_len;
 
 	msg = msgb_alloc_headroom(1024, 128, "GSM 04.80");
 	if (!msg)
@@ -424,7 +434,12 @@ struct msgb *gsm0480_create_ussd_resp(uint8_t invoke_id, uint8_t trans_id, const
 	/* First put the payload text into the message */
 	ptr8 = msgb_put(msg, 0);
 	response_len = gsm_7bit_encode(ptr8, text);
-	msgb_put(msg, response_len);
+	octet_len = response_len*7/8;
+	if (response_len*7%8 != 0)
+		octet_len++;
+	/* Warning, response_len indicates the amount of septets
+	 * (characters), we need amount of octets occupied */
+	msgb_put(msg, octet_len);
 
 	/* Then wrap it as an Octet String */
 	msgb_wrap_with_TL(msg, ASN1_OCTET_STRING_TAG);
diff --git a/src/gsm/gsm_utils.c b/src/gsm/gsm_utils.c
index 9569cf32..fa77eae4 100644
--- a/src/gsm/gsm_utils.c
+++ b/src/gsm/gsm_utils.c
@@ -241,6 +241,12 @@ int gsm_septets2octets(uint8_t *result, const uint8_t *rdata, uint8_t septet_len
 		result[z++] = cb;
 		shift++;
 	}
+	/* To avoid the situation where the receiving entity confuses 7 binary
+	 * zero pad bits as the @ character, the carriage return or <CR>
+	 * character (defined in subclause 7.1.1) shall be used for padding in
+	 * this situation. */
+	if (shift == 7)
+		result[z - 1] |= 0x1a;
 
 	free(data);
 
diff --git a/src/gsm/lapd_core.c b/src/gsm/lapd_core.c
index 116e3116..68b5e784 100644
--- a/src/gsm/lapd_core.c
+++ b/src/gsm/lapd_core.c
@@ -826,14 +826,23 @@ static int lapd_rx_u(struct msgb *msg, struct lapd_msg_ctx *lctx)
 			 * yet received UA or another mobile (collision) tries
 			 * to establish connection. The mobile must receive
 			 * UA again. */
-			if (!dl->cont_res && dl->v_send != dl->v_recv) {
-				LOGP(DLLAPD, LOGL_INFO, "Remote reestablish\n");
-				mdl_error(MDL_CAUSE_SABM_MF, lctx);
+			/* 5.4.2.1 */
+			if (!length) {
+				/* If no content resolution, this is a
+				 * re-establishment. */
+				LOGP(DLLAPD, LOGL_INFO,
+					"Remote reestablish\n");
 				break;
 			}
+			if (!dl->cont_res) {
+				LOGP(DLLAPD, LOGL_INFO, "SABM command not "
+						"allowed in this state\n");
+				mdl_error(MDL_CAUSE_SABM_MF, lctx);
+				msgb_free(msg);
+				return 0;
+			}
 			/* Ignore SABM if content differs from first SABM. */
-			if (dl->mode == LAPD_MODE_NETWORK && length
-			 && dl->cont_res) {
+			if (dl->mode == LAPD_MODE_NETWORK && length) {
 #ifdef TEST_CONTENT_RESOLUTION_NETWORK
 				dl->cont_res->data[0] ^= 0x01;
 #endif
diff --git a/src/gsm/lapdm.c b/src/gsm/lapdm.c
index 71045aa1..52290cbe 100644
--- a/src/gsm/lapdm.c
+++ b/src/gsm/lapdm.c
@@ -407,6 +407,11 @@ static int send_rslms_dlsap(struct osmo_dlsap_prim *dp,
 
 	switch (OSMO_PRIM_HDR(&dp->oph)) {
 	case OSMO_PRIM(PRIM_DL_EST, PRIM_OP_INDICATION):
+		if (dp->oph.msg && dp->oph.msg->len == 0) {
+			/* omit L3 info by freeing message */
+			msgb_free(dp->oph.msg);
+			dp->oph.msg = NULL;
+		}
 		rll_msg = RSL_MT_EST_IND;
 		break;
 	case OSMO_PRIM(PRIM_DL_EST, PRIM_OP_CONFIRM):
@@ -817,10 +822,10 @@ static int rslms_rx_rll_est_req(struct msgb *msg, struct lapdm_datalink *dl)
 static int rslms_rx_rll_udata_req(struct msgb *msg, struct lapdm_datalink *dl)
 {
 	struct lapdm_entity *le = dl->entity;
-	int ui_bts = (le->mode == LAPDM_MODE_BTS);
 	struct abis_rsl_rll_hdr *rllh = msgb_l2(msg);
 	uint8_t chan_nr = rllh->chan_nr;
 	uint8_t link_id = rllh->link_id;
+	int ui_bts = (le->mode == LAPDM_MODE_BTS && (link_id & 0x40));
 	uint8_t sapi = link_id & 7;
 	struct tlv_parsed tv;
 	int length;
@@ -844,9 +849,10 @@ static int rslms_rx_rll_udata_req(struct msgb *msg, struct lapdm_datalink *dl)
 	msg->l3h = (uint8_t *) TLVP_VAL(&tv, RSL_IE_L3_INFO);
 	length = TLVP_LEN(&tv, RSL_IE_L3_INFO);
 	/* check if the layer3 message length exceeds N201 */
-	if (length + 4 + !ui_bts > 23) {
+	if (length + ((link_id & 0x40) ? 4 : 2) + !ui_bts > 23) {
 		LOGP(DLLAPD, LOGL_ERROR, "frame too large: %d > N201(%d) "
-			"(discarding)\n", length, 18 + ui_bts);
+			"(discarding)\n", length,
+			((link_id & 0x40) ? 18 : 20) + ui_bts);
 		msgb_free(msg);
 		return -EIO;
 	}
@@ -860,13 +866,16 @@ static int rslms_rx_rll_udata_req(struct msgb *msg, struct lapdm_datalink *dl)
 	msg->tail = msg->l3h + length;
 
 	/* Push L1 + LAPDm header on msgb */
-	msg->l2h = msgb_push(msg, 4 + !ui_bts);
-	msg->l2h[0] = le->tx_power;
-	msg->l2h[1] = le->ta;
-	msg->l2h[2] = LAPDm_ADDR(LAPDm_LPD_NORMAL, sapi, dl->dl.cr.loc2rem.cmd);
-	msg->l2h[3] = LAPDm_CTRL_U(LAPDm_U_UI, 0);
+	msg->l2h = msgb_push(msg, 2 + !ui_bts);
+	msg->l2h[0] = LAPDm_ADDR(LAPDm_LPD_NORMAL, sapi, dl->dl.cr.loc2rem.cmd);
+	msg->l2h[1] = LAPDm_CTRL_U(LAPDm_U_UI, 0);
 	if (!ui_bts)
-		msg->l2h[4] = LAPDm_LEN(length);
+		msg->l2h[2] = LAPDm_LEN(length);
+	if (link_id & 0x40) {
+		msg->l2h = msgb_push(msg, 2);
+		msg->l2h[0] = le->tx_power;
+		msg->l2h[1] = le->ta;
+	}
 
 	/* Tramsmit */
 	return tx_ph_data_enqueue(dl, msg, chan_nr, link_id, 23);
@@ -1083,7 +1092,6 @@ static int rslms_rx_rll(struct msgb *msg, struct lapdm_channel *lc)
 	}
 
 	switch (msg_type) {
-	case RSL_MT_UNIT_DATA_REQ:
 	case RSL_MT_DATA_REQ:
 	case RSL_MT_SUSP_REQ:
 	case RSL_MT_REL_REQ:
diff --git a/tests/lapd/lapd_test.c b/tests/lapd/lapd_test.c
index 1f986bc8..1a06cb1c 100644
--- a/tests/lapd/lapd_test.c
+++ b/tests/lapd/lapd_test.c
@@ -179,6 +179,32 @@ static int send_sabm(struct lapdm_channel *chan, int second_ms)
 	return 0;
 }
 
+static int send_sabm(struct lapdm_channel *chan, int second_ms)
+{
+	struct osmo_phsap_prim pp;
+	struct msgb *msg;
+	int rc;
+
+	msg = msgb_alloc_headroom(128, 64, "PH-DATA.ind");
+	osmo_prim_init(&pp.oph, SAP_GSM_PH, PRIM_PH_DATA,
+			PRIM_OP_INDICATION, msg);
+	/* copy over actual MAC block */
+	msg->l2h = msgb_put(msg, 23);
+	msg->l2h[0] = 0x01;
+	msg->l2h[1] = 0x3f;
+	msg->l2h[2] = 0x01 | (sizeof(cm) << 2);
+	memcpy(msg->l2h + 3, cm_padded, sizeof(cm_padded));
+	msg->l2h[3] += second_ms; /* alter message, for second mobile */
+
+	/* LAPDm requires those... */
+	pp.u.data.chan_nr = 0;
+	pp.u.data.link_id = 0;
+        /* feed into the LAPDm code of libosmogsm */
+        rc = lapdm_phsap_up(&pp.oph, &chan->lapdm_dcch);
+	ASSERT(rc == 0 || rc == -EBUSY);
+	return 0;
+}
+
 /*
  * I get called from the LAPDm code when something was sent my way...
  */
@@ -417,6 +443,49 @@ static void test_lapdm_contention_resolution()
 	lapdm_channel_exit(&bts_to_ms_channel);
 }
 
+static void test_lapdm_contention_resolution()
+{
+	printf("I test contention resultion by having two mobiles collide and "
+		"first mobile repeating SABM.\n");
+
+	int rc;
+	struct lapdm_polling_state test_state;
+	struct osmo_phsap_prim pp;
+
+	/* Configure LAPDm on both sides */
+	struct lapdm_channel bts_to_ms_channel;
+	memset(&bts_to_ms_channel, 0, sizeof(bts_to_ms_channel));
+
+	memset(&test_state, 0, sizeof(test_state));
+	test_state.bts = &bts_to_ms_channel;
+
+	/* BTS to MS in polling mode */
+	lapdm_channel_init(&bts_to_ms_channel, LAPDM_MODE_BTS);
+	lapdm_channel_set_flags(&bts_to_ms_channel, LAPDM_ENT_F_POLLING_ONLY);
+	lapdm_channel_set_l1(&bts_to_ms_channel, NULL, &test_state);
+	lapdm_channel_set_l3(&bts_to_ms_channel, bts_to_ms_tx_cb, &test_state);
+
+	/* Send SABM MS 1, we must get UA */
+	send_sabm(&bts_to_ms_channel, 0);
+	rc = lapdm_phsap_dequeue_prim(&bts_to_ms_channel.lapdm_dcch, &pp);
+	CHECK_RC(rc);
+	ASSERT(memcmp(pp.oph.msg->l2h, ua, ARRAY_SIZE(ua)) == 0);
+
+	/* Send SABM MS 2, we must get nothing, due to collision */
+	send_sabm(&bts_to_ms_channel, 1);
+	rc = lapdm_phsap_dequeue_prim(&bts_to_ms_channel.lapdm_dcch, &pp);
+	ASSERT(rc == -ENODEV);
+
+	/* Send SABM MS 1 again, we must get UA gain */
+	send_sabm(&bts_to_ms_channel, 0);
+	rc = lapdm_phsap_dequeue_prim(&bts_to_ms_channel.lapdm_dcch, &pp);
+	CHECK_RC(rc);
+	ASSERT(memcmp(pp.oph.msg->l2h, ua, ARRAY_SIZE(ua)) == 0);
+
+	/* clean up */
+	lapdm_channel_exit(&bts_to_ms_channel);
+}
+
 int main(int argc, char **argv)
 {
 	osmo_init_logging(&info);