diff --git a/openbsc/src/input/lapd.c b/openbsc/src/input/lapd.c
index f8a5a7f..bd52cf8 100644
--- a/openbsc/src/input/lapd.c
+++ b/openbsc/src/input/lapd.c
@@ -154,13 +154,22 @@
 	switch (mt) {
 	case 0x01:{		// identity request
 			int tei = action;
+			uint8_t resp[8];
+			lapd_tei_t *teip;
+
 			DEBUGP(DMI, "tei mgmt: identity request, accepting "
 				   "tei %d\n", tei);
-			uint8_t resp[8];
 			memmove(resp, "\xfe\xff\x03\x0f\x00\x00\x02\x00", 8);
 			resp[7] = (tei << 1) | 1;
 			lapd_transmit_cb(resp, 8, cbdata);
-			lapd_tei_t *teip = teip_from_tei(tei);
+
+			teip = teip_from_tei(tei);
+			if (!teip) {
+				LOGP(DMI, LOGL_NOTICE, "Message for unknown "
+				     "TEI %u, LAPD code supports only "
+				     "TEI 25, 1, 2\n", tei);
+				return;
+			}
 			if (teip->state == LAPD_TEI_NONE)
 				lapd_tei_set_state(teip, LAPD_TEI_ASSIGNED);
 			break;
@@ -174,10 +183,13 @@
 uint8_t *lapd_receive(uint8_t * data, int len, int *ilen, lapd_mph_type * prim,
 		      void *cbdata)
 {
-#if 0
-	DEBUGP(DMI, "receive %p, %d\n", data, len);
-	hexdump(data, len);
-#endif
+	uint8_t sapi, cr, tei, command;
+	int pf, ns, nr;
+	uint8_t *contents;
+	lapd_tei_t *teip;
+
+	uint8_t resp[8];
+	int l = 0;
 
 	*ilen = 0;
 	*prim = 0;
@@ -193,10 +205,10 @@
 		return NULL;
 	};
 
-	int sapi = data[0] >> 2;
-	int cr = (data[0] >> 1) & 1;
-	int tei = data[1] >> 1;
-	int command = network_side ^ cr;
+	sapi = data[0] >> 2;
+	cr = (data[0] >> 1) & 1;
+	tei = data[1] >> 1;
+	command = network_side ^ cr;
 	//DEBUGP(DMI, "  address sapi %x tei %d cmd %d cr %d\n", sapi, tei, command, cr);
 
 	if (len < 3) {
@@ -206,9 +218,9 @@
 
 	lapd_msg_type typ = 0;
 	lapd_cmd_type cmd = 0;
-	int pf = -1;
-	int ns = -1;
-	int nr = -1;
+	pf = -1;
+	ns = -1;
+	nr = -1;
 	if ((data[2] & 1) == 0) {
 		typ = LAPD_TYPE_I;
 		assert(len >= 4);
@@ -269,159 +281,135 @@
 		};
 	};
 
-	uint8_t *contents = &data[4];
+	contents = &data[4];
 	if (typ == LAPD_TYPE_U)
 		contents--;
 	*ilen = len - (contents - data);
 
-	lapd_tei_t *teip = teip_from_tei(tei);
 	if (tei == 127)
 		lapd_tei_receive(contents, *ilen, cbdata);
 
+	teip = teip_from_tei(tei);
+
 	DEBUGP(DMI, "<- %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, teip ? teip->vs : -1, teip ? teip->va : -1,
 	     teip ? teip->vr : -1, len);
 
-	if (teip) {
-		switch (cmd) {
-		case LAPD_CMD_I:{
-				if (ns != teip->vr) {
-					DEBUGP(DMI, "ns %d != vr %d\n", ns,
-						   teip->vr);
-					if (ns == ((teip->vr - 1) & 0x7f)) {
-						DEBUGP(DMI, "DOUBLE FRAME, ignoring\n");
-						cmd = 0;	// ignore
-					} else {
-						assert(0);
-					};
-				} else {
-					//printf("IN SEQUENCE\n");
-					teip->vr = (ns + 1) & 0x7f;	// FIXME: hack!
-				};
+	if (!teip) {
+		LOGP(DMI, LOGL_NOTICE, "Unknown TEI %u\n", tei);
+		return NULL;
+	}
 
-				break;
-			}
-		case LAPD_CMD_UI:
-			break;
-		case LAPD_CMD_SABME:{
-				teip->vs = 0;
-				teip->vr = 0;
-				teip->va = 0;
-
-				// ua
-				uint8_t resp[8];
-				int l = 0;
-				resp[l++] = data[0];
-				resp[l++] = (tei << 1) | 1;
-				resp[l++] = 0x73;
-				lapd_transmit_cb(resp, l, 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(DMI, "rr in strange state, send rej\n");
-
-						// rej
-						uint8_t resp[8];
-						int l = 0;
-						resp[l++] =
-						    (teip->
-						     sapi << 2) | (network_side
-								   ? 0 : 2);
-						resp[l++] = (tei << 1) | 1;
-						resp[l++] = 0x09;	//rej
-						resp[l++] =
-						    ((teip->vr + 1) << 1) | 0;
-						lapd_transmit_cb(resp, l,
-								 cbdata);
-						pf = 0;	// dont reply
-#endif
-					};
-				};
-
-				*prim = LAPD_MPH_ACTIVATE_IND;
-				break;
-			}
-		case LAPD_CMD_RR:{
-				teip->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(DMI, "rr in strange "
-						       "state, send rej\n");
-
-						// rej
-						uint8_t resp[8];
-						int l = 0;
-						resp[l++] =
-						    (teip->
-						     sapi << 2) | (network_side
-								   ? 0 : 2);
-						resp[l++] = (tei << 1) | 1;
-						resp[l++] = 0x09;	//rej
-						resp[l++] =
-						    ((teip->vr + 1) << 1) | 0;
-						lapd_transmit_cb(resp, l,
-								 cbdata);
-						pf = 0;	// dont reply
-#endif
-					};
-				};
-#endif
-				if (pf) {
-					// interrogating us, send rr
-					uint8_t resp[8];
-					int l = 0;
-					resp[l++] = data[0];
-					resp[l++] = (tei << 1) | 1;
-					resp[l++] = 0x01;	// rr
-					resp[l++] = (LAPD_NR(teip) << 1) | (data[3] & 1);	// pf bit from req
-
-					lapd_transmit_cb(resp, l, 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
-				DEBUGP(DMI, "frame reject, ignoring\n");
+	switch (cmd) {
+	case LAPD_CMD_I:
+		if (ns != teip->vr) {
+			DEBUGP(DMI, "ns %d != vr %d\n", ns, teip->vr);
+			if (ns == ((teip->vr - 1) & 0x7f)) {
+				DEBUGP(DMI, "DOUBLE FRAME, ignoring\n");
+				cmd = 0;	// ignore
+			} else {
 				assert(0);
-				break;
-			}
-		case LAPD_CMD_DISC:{
-				// disconnect
-				uint8_t resp[8];
-				int l = 0;
-				resp[l++] = data[0];
-				resp[l++] = (tei << 1) | 1;
-				resp[l++] = 0x73;
-				lapd_transmit_cb(resp, l, cbdata);
-				lapd_tei_set_state(teip, LAPD_TEI_NONE);
-				break;
-			}
-		default:
-			DEBUGP(DMI, "unknown cmd for tei %d (cmd %x)\n", tei,
-				   cmd);
-			assert(0);
+			};
+		} else {
+			//printf("IN SEQUENCE\n");
+			teip->vr = (ns + 1) & 0x7f;	// FIXME: hack!
 		};
-	};
+
+		break;
+	case LAPD_CMD_UI:
+		break;
+	case LAPD_CMD_SABME:
+		teip->vs = 0;
+		teip->vr = 0;
+		teip->va = 0;
+
+		// ua
+		resp[l++] = data[0];
+		resp[l++] = (tei << 1) | 1;
+		resp[l++] = 0x73;
+		lapd_transmit_cb(resp, l, 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(DMI, "rr in strange state, send rej\n");
+
+				// rej
+				resp[l++] = (teip-> sapi << 2) | (network_side ? 0 : 2);
+				resp[l++] = (tei << 1) | 1;
+				resp[l++] = 0x09;	//rej
+				resp[l++] = ((teip->vr + 1) << 1) | 0;
+				lapd_transmit_cb(resp, l, cbdata);
+				pf = 0;	// dont reply
+#endif
+			};
+		};
+
+		*prim = LAPD_MPH_ACTIVATE_IND;
+		break;
+	case LAPD_CMD_RR:
+		teip->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(DMI, "rr in strange " "state, send rej\n");
+
+				// rej
+				resp[l++] = (teip-> sapi << 2) | (network_side ? 0 : 2);
+				resp[l++] = (tei << 1) | 1;
+				resp[l++] = 0x09;	//rej
+				resp[l++] =
+				    ((teip->vr + 1) << 1) | 0;
+				lapd_transmit_cb(resp, l,
+						 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(teip) << 1) | (data[3] & 1);	// pf bit from req
+
+			lapd_transmit_cb(resp, l, 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
+		DEBUGP(DMI, "frame reject, ignoring\n");
+		assert(0);
+		break;
+	case LAPD_CMD_DISC:
+		// disconnect
+		resp[l++] = data[0];
+		resp[l++] = (tei << 1) | 1;
+		resp[l++] = 0x73;
+		lapd_transmit_cb(resp, l, cbdata);
+		lapd_tei_set_state(teip, LAPD_TEI_NONE);
+		break;
+	default:
+		DEBUGP(DMI, "unknown cmd for tei %d (cmd %x)\n", tei, cmd);
+		assert(0);
+	}
 
 	//if ((*prim == 0) && (ilen > 0) && (typ != LAPD_TYPE_S)) {
 	//if (cmd == LAPD_CMD_I) {
@@ -432,8 +420,6 @@
 
 		// interrogating us, send rr
 		DEBUGP(DMI, "Sending RR response\n");
-		uint8_t resp[8];
-		int l = 0;
 		resp[l++] = data[0];
 		resp[l++] = (tei << 1) | 1;
 		resp[l++] = 0x01;	// rr