Iniatial gsmtap support

This requires a wireshark with support for passing the new gsmtap V5
format into the existing wireshark dissectors for V5.

Makefile

@@ -4,7 +4,7 @@ CFLAGS=-Wall -g -DPACKAGE_VERSION=\"0.0\"
 %.o: %.c
 	$(CC) $(CFLAGS) -o $@ -c $^ -I/usr/local/include/
 
-v5le: main.o logging.o v5x_data.o lapv5.o v5x_protocol.o
+v5le: main.o logging.o v5x_data.o lapv5.o v5x_protocol.o v51_le_ctrl.o
 	$(CC) -o $@ $^ $(LIBS)
 
 clean:

lapv5.c

@@ -24,6 +24,7 @@
 
 #include "v5x_internal.h"
 #include "v5x_protocol.h"
+#include "logging.h"
 #include "lapv5.h"
 
 #include <stdio.h>
@@ -31,15 +32,14 @@
 #include <errno.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>
 #include <osmocom/abis/lapd_pcap.h>
 
-#define LAPD_ADDR2(sapi, cr) ((((sapi) & 0x3f) << 2) | (((cr) & 0x1) << 1))
-#define LAPD_ADDR3(tei) ((((tei) & 0x7f) << 1) | 0x1)
+#define LAPD_ADDR1(sapi, cr) ((((sapi) & 0x3f) << 2) | (((cr) & 0x1) << 1))
+#define LAPD_ADDR2(tei) ((((tei) & 0x7f) << 1) | 0x1)
 
 #define LAPD_ADDR_SAPI(addr) ((addr) >> 2)
 #define LAPD_ADDR_CR(addr) (((addr) >> 1) & 0x1)
@@ -135,6 +135,7 @@ static struct lapv5_sap *lapv5_sap_alloc(struct lapv5_instance *li, uint16_t v5d
 	LOGP(DLLAPD, LOGL_NOTICE, "(%s): LAPV5 Allocating SAP for V5_DLADDR=%u (dl=%p, sap=%p)\n",
 	     name, v5dladdr, &sap->dl, sap);
 
+	sap->li = li;
 	sap->dladdr = v5dladdr;
 	dl = &sap->dl;
 	profile = &li->profile;
@@ -152,8 +153,8 @@ static struct lapv5_sap *lapv5_sap_alloc(struct lapv5_instance *li, uint16_t v5d
 	dl->t200_sec = profile->t200_sec; dl->t200_usec = profile->t200_usec;
 	dl->t203_sec = profile->t203_sec; dl->t203_usec = profile->t203_usec;
 	dl->lctx.dl = &sap->dl;
-	dl->lctx.sapi = v5dladdr & 0xff;
-	dl->lctx.tei = v5dladdr >> 8;
+	dl->lctx.sapi = v5dladdr >> 7;
+	dl->lctx.tei = v5dladdr & 0x7f;
 	dl->lctx.n201 = profile->n201;
 
 	lapd_set_mode(&sap->dl, (li->network_side) ? LAPD_MODE_NETWORK : LAPD_MODE_USER);
@@ -181,33 +182,29 @@ int lapv5_ph_data_ind(struct lapv5_instance *li, struct msgb *msg, int *error)
 	struct lapd_msg_ctx lctx;
 	struct lapv5_sap *sap;
 	uint16_t dladdr;
-	bool is_isdn;
 	int i;
 
 	/* write to PCAP file, if enabled. */
 	osmo_pcap_lapd_write(li->pcap_fd, OSMO_LAPD_PCAP_INPUT, msg);
 
+	msgb_pull(msg, msg->l2h - msg->data);
+
 	LOGLI(li, LOGL_DEBUG, "RX: %s\n", osmo_hexdump(msg->data, msg->len));
 	if (msg->len < 2) {
 		LOGLI(li, LOGL_ERROR, "LAPV5 frame receive len %d < 2, ignoring\n", msg->len);
 		*error = LAPD_ERR_BAD_LEN;
+		msgb_free(msg);
 		return -EINVAL;
 	};
-	msg->l2h = msg->data;
 
 	memset(&lctx, 0, sizeof(lctx));
 
 	i = 0;
 	/* adress field */
-	dladdr = v51_l3_addr_dec(msg->l2h[0] << 8 | msg->l2h[1], &is_isdn);
-	if (!is_isdn) {
-		LOGLI(li, LOGL_ERROR, "LAPV5 frame with single-octet addr not permitted\n");
-		*error = LAPD_ERR_BAD_LEN;
-		return -EINVAL;
-	};
+	dladdr = ((msg->l2h[0] & 0xfe) << 5) | (msg->l2h[1] >> 1);
 	lctx.lpd = 0;
-	lctx.tei = dladdr >> 8;
-	lctx.sapi = dladdr & 0xff;
+	lctx.sapi = dladdr >> 7;
+	lctx.tei = dladdr & 0x7f;
 	lctx.cr = LAPD_ADDR_CR(msg->l2h[0]);
 	i += 2;
 
@@ -219,6 +216,7 @@ int lapv5_ph_data_ind(struct lapv5_instance *li, struct msgb *msg, int *error)
 		if (msg->len < 4) {
 			LOGLI(li, LOGL_ERROR, "LAPV5 I frame receive len %d < 4, ignoring\n", msg->len);
 			*error = LAPD_ERR_BAD_LEN;
+			msgb_free(msg);
 			return -EINVAL;
 		}
 		lctx.n_recv = LAPD_CTRL_Nr(msg->l2h[i]);
@@ -230,6 +228,7 @@ int lapv5_ph_data_ind(struct lapv5_instance *li, struct msgb *msg, int *error)
 		if (msg->len < 4 && i == 3) {
 			LOGLI(li, LOGL_ERROR, "LAPV5 S frame receive len %d < 4, ignoring\n", msg->len);
 			*error = LAPD_ERR_BAD_LEN;
+			msgb_free(msg);
 			return -EINVAL;
 		}
 		lctx.n_recv = LAPD_CTRL_Nr(msg->l2h[i]);
@@ -319,7 +318,7 @@ int lapv5_dl_rel_req(struct lapv5_instance *li, uint16_t dladdr)
 }
 
 /* Transmit Data (DL-DATA request) on the given LAPD Instance / DLADDR */
-void lapv5_dl_data_req(struct lapv5_instance *li, uint8_t dladdr, struct msgb *msg)
+int lapv5_dl_data_req(struct lapv5_instance *li, uint16_t dladdr, struct msgb *msg)
 {
 	struct lapv5_sap *sap;
 	struct osmo_dlsap_prim dp;
@@ -328,15 +327,15 @@ void lapv5_dl_data_req(struct lapv5_instance *li, uint8_t dladdr, struct msgb *m
 	if (!sap) {
 		LOGLI(li, LOGL_INFO, "LAPV5 Tx on unknown DLADDR=%u\n", dladdr);
 		msgb_free(msg);
-		return;
+		return -EINVAL;
 	}
 
 	/* prepare prim */
 	msg->l3h = msg->data;
-        osmo_prim_init(&dp.oph, 0, PRIM_DL_DATA, PRIM_OP_REQUEST, msg);
+	osmo_prim_init(&dp.oph, 0, PRIM_DL_DATA, PRIM_OP_REQUEST, msg);
 
-        /* send to L2 */
-        lapd_recv_dlsap(&dp, &sap->dl.lctx);
+	/* send to L2 */
+	return lapd_recv_dlsap(&dp, &sap->dl.lctx);
 };
 
 static int send_ph_data_req(struct lapd_msg_ctx *lctx, struct msgb *msg)
@@ -368,8 +367,8 @@ static int send_ph_data_req(struct lapd_msg_ctx *lctx, struct msgb *msg)
 	}
 	/* address field */
 	msg->l2h = msgb_push(msg, 2);
-	msg->l2h[0] = LAPD_ADDR2(lctx->sapi, lctx->cr);
-	msg->l2h[1] = LAPD_ADDR3(lctx->tei);
+	msg->l2h[0] = LAPD_ADDR1(lctx->sapi, lctx->cr);
+	msg->l2h[1] = LAPD_ADDR2(lctx->tei);
 
 	/* write to PCAP file, if enabled. */
 	osmo_pcap_lapd_write(li->pcap_fd, OSMO_LAPD_PCAP_OUTPUT, msg);
@@ -392,7 +391,7 @@ static int send_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx)
 
 	li = sap->li;
 
-	dladdr = lctx->tei << 8 | lctx->sapi;
+	dladdr = lctx->sapi << 7 | lctx->tei;
 
 	switch (dp->oph.primitive) {
 	case PRIM_DL_EST:
@@ -414,8 +413,8 @@ static int send_dlsap(struct osmo_dlsap_prim *dp, struct lapd_msg_ctx *lctx)
 
 /* Allocate a new LAPV5 instance */
 struct lapv5_instance *lapv5_instance_alloc(int network_side,
-	void (*ph_data_req_cb)(struct msgb *msg, void *cbdata), void *ph_data_req_cbdata,
-	void (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata), void *dl_receive_cbdata,
+	int (*ph_data_req_cb)(struct msgb *msg, void *cbdata), void *ph_data_req_cbdata,
+	int (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata), void *dl_receive_cbdata,
 	const struct lapd_profile *profile, const char *name)
 {
 	struct lapv5_instance *li;
@@ -470,23 +469,27 @@ void lapv5_instance_free(struct lapv5_instance *li)
  * while the last octet (before msg->tail) points to the last FCS octet. */
 int lapv5ef_rx(struct v5x_link *link, struct msgb *msg)
 {
-	struct lapd_datalink *dl;
+	struct lapv5_instance *li;
 	uint16_t efaddr, efaddr_enc;
 	bool is_isdn;
+	int error;
 
 	msg->l1h = msg->data;
 
 	if (msgb_length(msg) < 2) {
+		LOGP(DV5EF, LOGL_ERROR, "Frame too short.\n");
 		msgb_free(msg);
 		return -EINVAL;
 	}
 
-	msg->l2h = msg->l1h + 2;
-	efaddr_enc = msg->l1h[0] << 8 | msg->l1h[1];
+	efaddr_enc = msg->l2h[0] << 8 | msg->l2h[1];
+	msg->l2h += 2;
+	msgb_pull(msg, msg->l2h - msg->data);
 	efaddr = v51_l3_addr_dec(efaddr_enc, &is_isdn);
 
+
 	if (!is_isdn) {
-		/* EFaddr are structured like isdn-type L3 Address */
+		LOGP(DV5EF, LOGL_ERROR, "EFaddr are structured like isdn-type L3 Address.\n");
 		msgb_free(msg);
 		return -EINVAL;
 	}
@@ -494,32 +497,33 @@ int lapv5ef_rx(struct v5x_link *link, struct msgb *msg)
 	switch (efaddr) {
 	case V51_DLADDR_PSTN:
 		/* hand-over to LAPD-DL instance for PSTN */
-		dl = &link->interface->pstn.dl;
+		li = link->interface->pstn.li;
 		/* TODO */
 		break;
 	case V51_DLADDR_CTRL:
 		/* hand-over to LAPD-DL instance for CTRL */
 		/* TODO */
-		dl = &link->interface->control.dl;
+		li = link->interface->control.li;
 		break;
 	case V52_DLADDR_BCC:
-		dl = &link->interface->bcc.dl;
+		li = link->interface->bcc.li;
 		/* TOOD: implement V5.2 */
 		msgb_free(msg);
 		break;
 	case V52_DLADDR_PROTECTION:
-		dl = &link->interface->protection[0].dl;
+		li = link->interface->protection[0].li;
 		/* TOOD: implement V5.2 */
 		msgb_free(msg);
 		break;
 	case V52_DLADDR_LCP:
-		dl = &link->interface->lcp.dl;
+		li = link->interface->lcp.li;
 		/* TOOD: implement V5.2 */
 		msgb_free(msg);
 		break;
 	default:
 		if (efaddr >= 8176) {
 			/* reserved as per Section 9.2.2.2 of G.964 */
+			LOGP(DV5EF, LOGL_ERROR, "No LAPV5 protocol for EFaddr %d.\n", efaddr);
 			msgb_free(msg);
 			return -EINVAL;
 		}
@@ -527,5 +531,13 @@ int lapv5ef_rx(struct v5x_link *link, struct msgb *msg)
 		/* TODO */
 	}
 
+	if (!li) {
+		LOGP(DV5EF, LOGL_ERROR, "No LAPV5 instance for EFaddr %d created.\n", efaddr);
+		msgb_free(msg);
+		return -EINVAL;
+	}
+
+	lapv5_ph_data_ind(li, msg, &error);
+
 	return 0;
 }

lapv5.h

@@ -6,9 +6,9 @@ struct lapv5_instance {
 	struct llist_head list;		/* list of LAPV5 instances */
 	bool network_side;
 
-	void (*ph_data_req_cb)(struct msgb *msg, void *cbdata);
+	int (*ph_data_req_cb)(struct msgb *msg, void *cbdata);
 	void *ph_data_req_cbdata;
-	void (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata);
+	int (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata);
 	void *dl_receive_cbdata;
 
 	struct lapd_profile profile;	/* must be a copy */
@@ -18,15 +18,19 @@ struct lapv5_instance {
 	char *name;			/* human-readable name */
 };
 
+extern const struct lapd_profile lapd_profile_lapv5dl;
+
 int lapv5ef_rx(struct v5x_link *link, struct msgb *msg);
 
 int lapv5_dl_est_req(struct lapv5_instance *li, uint16_t dladdr);
 int lapv5_dl_rel_req(struct lapv5_instance *li, uint16_t dladdr);
-void lapv5_dl_data_req(struct lapv5_instance *li, uint8_t dladdr, struct msgb *msg);
+int lapv5_dl_data_req(struct lapv5_instance *li, uint16_t dladdr, struct msgb *msg);
 
 int lapv5_ph_data_ind(struct lapv5_instance *li, struct msgb *msg, int *error);
 
 struct lapv5_instance *lapv5_instance_alloc(int network_side,
-	void (*ph_data_req_cb)(struct msgb *msg, void *cbdata), void *ph_data_req_cbdata,
-	void (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata), void *dl_receive_cbdata,
+	int (*ph_data_req_cb)(struct msgb *msg, void *cbdata), void *ph_data_req_cbdata,
+	int (*dl_receive_cb)(struct osmo_dlsap_prim *odp, uint16_t dladdr, void *rx_cbdata), void *dl_receive_cbdata,
 	const struct lapd_profile *profile, const char *name);
+
+int lapv5ef_rx(struct v5x_link *link, struct msgb *msg);

main.c

@@ -35,6 +35,8 @@
 #include <osmocom/core/stats.h>
 #include <osmocom/core/rate_ctr.h>
 #include <osmocom/core/socket.h>
+#include <osmocom/core/gsmtap.h>
+#include <osmocom/core/gsmtap_util.h>
 
 #include <osmocom/ctrl/control_vty.h>
 
@@ -49,6 +51,10 @@
 #include <osmocom/abis/abis.h>
 
 #include "logging.h"
+#include "v5x_internal.h"
+#include "v5x_protocol.h"
+#include "v51_le_ctrl.h"
+#include "lapv5.h"
 
 #define _GNU_SOURCE
 #include <getopt.h>
@@ -58,7 +64,14 @@
 #define OSMO_CTRL_PORT_MGW 4267
 #endif
 
+/* only temporarily until this is in libosmocore gsmtap.h */
+#ifndef GSMTAP_E1T1_V5EF
+#define GSMTAP_E1T1_V5EF	0x06
+#endif
+
 void *tall_v5le_ctx = NULL;
+struct v5x_instance *v5i = NULL;
+static struct gsmtap_inst *g_gti;
 
 struct v5le_config {
 };
@@ -86,11 +99,41 @@ static const struct e1inp_line_ops v5le_e1_line_ops = {
 	.sign_link = v5le_rx_sign,
 };
 
+#include "v5x_protocol.h"
+/* data L1 -> L2 */
 static void hdlc_rx_cb(struct e1inp_ts *ts, struct msgb *msg)
 {
-	LOGP(DLINP, LOGL_NOTICE, "HDLC Rx: %s\n", msgb_hexdump(msg));
+#warning HACKING
+		struct v5x_interface *v5if = (struct v5x_interface *)v5i->interfaces.next;
+        struct v5x_user_port *v5up;
+	uint16_t dladdr;
 
-	msgb_free(msg);
+	LOGP(DLINP, LOGL_NOTICE, "L1->L2: %s\n", msgb_hexdump(msg));
+
+	/* send V5 data via gsmtap so wireshark can receive + decode it */
+	gsmtap_send_ex(g_gti, GSMTAP_TYPE_E1T1, GSMTAP_ARFCN_F_UPLINK, ts->num, GSMTAP_E1T1_V5EF,
+			0, 0, 0, 0, msgb_data(msg), msgb_length(msg));
+
+	lapv5ef_rx(v5if->primary_link, msg);
+}
+
+/* l2 -> L1 */
+int ph_data_req(struct msgb *msg, void *cbdata)
+{
+	msg->l2h = msgb_push(msg, 2);
+	msg->l2h[0] = msg->l2h[2] & 0xfd;
+	msg->l2h[1] = msg->l2h[3];
+	LOGP(DLINP, LOGL_NOTICE, "L2->L1: %s\n", msgb_hexdump(msg));
+
+#warning hacking
+		struct e1inp_line *e1_line = e1inp_line_find(0);
+		struct e1inp_ts *ts = &e1_line->ts[16-1];
+
+	/* send V5 data via gsmtap so wireshark can receive + decode it */
+	gsmtap_send_ex(g_gti, GSMTAP_TYPE_E1T1, 0, ts->num, GSMTAP_E1T1_V5EF,
+			0, 0, 0, 0, msgb_data(msg), msgb_length(msg));
+
+	return e1inp_ts_send_hdlc(ts, msg);
 }
 
 static int e1_init(void)
@@ -284,10 +327,17 @@ int main(int argc, char **argv)
 	rate_ctr_init(tall_v5le_ctx);
 	osmo_stats_init(tall_v5le_ctx);
 
+	// FIXME:
+	osmo_fsm_register(&v51_ctrl_fsm);
+
 	rc = vty_read_config_file(config_file, NULL);
 	if (rc < 0)
 		return rc;
 
+	g_gti = gsmtap_source_init("localhost", GSMTAP_UDP_PORT, 0);
+	OSMO_ASSERT(g_gti);
+	gsmtap_source_add_sink(g_gti);
+
 	rc = e1_init();
 	OSMO_ASSERT(rc == 0);
 
@@ -297,6 +347,23 @@ int main(int argc, char **argv)
 	if (rc < 0)
 		return rc;
 
+	/* create v5x instance */
+	v5i = v5x_instance_alloc(tall_v5le_ctx);
+	if (!v5i)
+		return -ENOMEM;
+
+	// FIXME: move this to VTY code
+	/* create v5x interface */
+	struct v5x_interface *v5if;
+	struct v5x_user_port *v5up;
+	uint32_t interface_id = 1;
+	uint8_t interface_variant = 1;
+	v5if = v5x_interface_alloc(v5i, V5X_DIALECT_V51, interface_id, interface_variant, ph_data_req);
+	if (!v5if)
+		return -ENOMEM;
+	v5up = v5x_user_port_create(v5if, 1001, 1);
+	if (!v5up)
+		return -ENOMEM;
 #if 0
 	cfg->ctrl = v5le_ctrl_interface_setup(cfg, ctrl_vty_get_bind_addr(), OSMO_CTRL_PORT_MGW);
 	if (!cfg->ctrl) {

v51_le_ctrl.c

@@ -20,197 +20,350 @@
  *
  */
 
-#include <unitstd.h>
+#include <unistd.h>
 #include <stdint.h>
 #include <errno.h>
+#include <assert.h>
+#include <arpa/inet.h>
 
-#include <osmocom/core/util.h>
+#include <osmocom/core/utils.h>
 #include <osmocom/core/msgb.h>
 #include <osmocom/gsm/tlv.h>
 
 #include "v5x_internal.h"
+#include "v5x_protocol.h"
+#include "v51_le_ctrl.h"
+#include "logging.h"
+
+#define S(x)	(1 << (x))
+
+#define TIMEOUT	1
+
+/* uncomment to test lost TX messages at the first transmission */
+#warning fixme
+//#define TEST_TX_FAILURE
 
 /* PSTN Protocol FSM */
-enum v51_le_ctrl_port_fsm_state {
+enum v51_le_ctrl_fsm_state {
 	V51_PSTN_PROT_S_OUT_OF_SERVICE,			/* LE0 */
-	V51_PSTN_PROT_S_NULL				/* LE1 */
-	V51_PSTN_PROT_S_PATH_INIT_BY_LE			/* LE2 */
-	V51_PSTN_PROT_S_PATH_INIT_BY_AN			/* LE3 */
-	V51_PSTN_PROT_S_PATH_ACTIVE			/* LE4 */
-	V51_PSTN_PROT_S_PATH_DISC_REQ			/* LE5 */
-	V51_PSTN_PROT_S_PORT_BLOCKED			/* LE6 */
+	V51_PSTN_PROT_S_NULL,				/* LE1 */
+	V51_PSTN_PROT_S_PATH_INIT_BY_LE,		/* LE2 */
+	V51_PSTN_PROT_S_PATH_INIT_BY_AN,		/* LE3 */
+	V51_PSTN_PROT_S_PATH_ACTIVE,			/* LE4 */
+	V51_PSTN_PROT_S_PATH_DISC_REQ,			/* LE5 */
+	V51_PSTN_PROT_S_PORT_BLOCKED,			/* LE6 */
 };
 
 /***********************************************************************
- * user port CTRL protocol FSM / G.964 Section 14
+ * port/common CTRL protocol FSM / G.964 Section 14
  ***********************************************************************/
 
-enum v51_ctrl_port_fsm_state {
-	V51_CTRL_PORT_ST_OUT_OF_SERVICE,
-	V51_CTRL_PORT_ST_IN_SERVICE,
-	V51_CTRL_PORT_AWAIT_PORT_ACK,
+enum v51_ctrl_fsm_state {
+	V51_CTRL_ST_OUT_OF_SERVICE = 0,
+	V51_CTRL_ST_IN_SERVICE,
+	V51_CTRL_ST_AWAIT_ACK,
 };
 
-enum v51_ctrl_port_fsm_event {
-	V51_CTRL_PE_MDU_START_TRAFFIC,
-	V51_CTRL_PE_MDU_STOP_TRAFFIC,
-	V51_CTRL_PE_MDU_CTRL,
-	V51_CTRL_PE_RX_PORT_CONTROL,
-	V51_CTRL_PE_RX_PORT_CONTROL_ACK,
+enum v51_ctrl_fsm_event {
+	V51_CTRL_EV_MDU_START_TRAFFIC,
+	V51_CTRL_EV_MDU_STOP_TRAFFIC,
+	V51_CTRL_EV_MDU_CTRL,
+	V51_CTRL_EV_RX_CONTROL,
+	V51_CTRL_EV_RX_CONTROL_ACK,
 };
 
-static const struct value_string v51_ctrl_port_fsm_event_names[] = {
-	{ V51_CTRL_PE_MDU_START_TRAFFIC,	"MDU-start_traffic" },
-	{ V51_CTRL_PE_MDU_STOP_TRAFFIC,		"MDU-stop_traffic" },
-	{ V51_CTRL_PE_MDU_CTRL,			"MDU-CTRL" },
-	{ V51_CTRL_PE_RX_PORT_CONTROL,		"PORT_CONTROL" },
-	{ V51_CTRL_PE_RX_PORT_CONTROL_ACK,	"PORT_CONTROL_ACK" },
+static const struct value_string v51_ctrl_fsm_event_names[] = {
+	{ V51_CTRL_EV_MDU_START_TRAFFIC,	"MDU-start_traffic" },
+	{ V51_CTRL_EV_MDU_STOP_TRAFFIC,		"MDU-stop_traffic" },
+	{ V51_CTRL_EV_MDU_CTRL,			"MDU-CTRL" },
+	{ V51_CTRL_EV_RX_CONTROL,		"CONTROL" },
+	{ V51_CTRL_EV_RX_CONTROL_ACK,		"CONTROL_ACK" },
 	{ 0, NULL }
 };
 
-static int v51_ctrl_port_fsm_timer_cb(struct osmo_fsm_inst *fi)
+static int blocked = 1;
+static void v51_mdu_ctrl(struct osmo_fsm_inst *fi, const struct tlv_parsed *tp)
 {
-	if () {
-		/* first expiry: repeat PORT_CONTROL; re-start T01 */
-	} else {
-		/* second expiry: send MDU-error_ind; go to IN_SERVICE */
-		osmo_fsm_inst_state_chg(fi, V51_CTRL_PORT_ST_IN_SERVICE);
+	struct v5x_ctrl_proto *ctrl = fi->priv;
+
+	LOGP(DV5CTRL, LOGL_NOTICE, "MDU-CTRL\n");
+
+	if (ctrl->v5if) {
+		puts("common control");
+		enum v51_ctrl_func_id cfi = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ID) & 0x7f;
+		printf("cfi=%d\n", cfi);
+		if (cfi == V51_CTRL_ID_REQUEST_VARIANT_AND_INTERFACE_ID) {
+			puts("request variant+id");
+			v51_snd_ctrl_common(ctrl->v5if, V51_CTRL_ID_VARIANT_AND_INTERFACE_ID, NULL, &ctrl->v5if->variant, &ctrl->v5if->id);
+		}
+		if (cfi == V51_CTRL_ID_RESTART_REQUEST) {
+			puts("request restart");
+			v51_snd_ctrl_common(ctrl->v5if, V51_CTRL_ID_RESTART_COMPLETE, NULL, NULL, NULL);
+		}
+	}
+	if (ctrl->v5up) {
+		puts("port control");
+		enum v51_ctrl_func_el cfe = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ELEMENT) & 0x7f;
+		printf("cfe=%d\n", cfe);
+		if (cfe == V51_CTRL_FE201_UNBLOCK) {
+			puts("request unblock");
+			if (blocked) {
+				blocked = 0;
+				v51_snd_ctrl_port(ctrl->v5up, V51_CTRL_FE201_UNBLOCK);
+			}
+		}
+		if (cfe == V51_CTRL_FE203_BLOCK) {
+			puts("request block");
+			if (!blocked) {
+				blocked = 1;
+				v51_snd_ctrl_port(ctrl->v5up, V51_CTRL_FE203_BLOCK);
+			}
+		}
 	}
 }
 
-static const struct osmo_fsm_state v51_ctrl_port_fsm_states[] = {
-	[V51_CTRL_PORT_ST_OUT_OF_SERVICE] = {
-		.name = "OUT_OF_SERVICE",
-		.in_event_mask = S(V51_CTRL_PE_MDU_START_TRAFFIC) |
-				 S(V51_CTRL_PE_MDU_STOP_TRAFFIC) |
-				 S(V51_CTRL_PE_MDU_STOP_TRAFFIC) |
-				 S(V51_CTRL_PE_RX_PORT_CONTROL) |
-				 S(V51_CTRL_PE_RX_PORT_CONTROL_ACK),
-		.out_state_mask = S(V51_CTRL_PORT_ST_IN_SERVICE),
-	},
-	[V51_CTRL_PORT_ST_IN_SERVICE] = {
-		.name = "IN_SERVICE",
-		.in_event_mask = S(V51_CTRL_PE_MDU_START_TRAFFIC) |
-				 S(V51_CTRL_PE_MDU_STOP_TRAFFIC) |
-				 S(V51_CTRL_PE_MDU_STOP_TRAFFIC) |
-				 S(V51_CTRL_PE_RX_PORT_CONTROL),
-
-		.out_state_mask = S(V51_CTRL_PORT_ST_OUT_OF_SERVICE) |
-				  S(V51_CTRL_PORT_AWAIT_PORT_ACK),
-	},
-	[V51_CTRL_PORT_AWAIT_PORT_ACK] = {
-		.name = "AWAIT_PORT_ACK",
-		.in_event_mask = S(V51_CTRL_PE_MDU_START_TRAFFIC) |
-				 S(V51_CTRL_PE_MDU_STOP_TRAFFIC) |
-				 S(V51_CTRL_PE_MDU_STOP_TRAFFIC) |
-				 S(V51_CTRL_PE_RX_PORT_CONTROL) |
-				 S(V51_CTRL_PE_RX_PORT_CONTROL_ACK),
-		.out_state_mask = S(V51_CTRL_PORT_ST_OUT_OF_SERVICE) |
-				  S(V51_CTRL_PORT_ST_IN_SERVICE),
-	},
-};
-
-static struct osmo_fsm v51_ctrl_port_fsm = {
-	.name = "V51_CTRL_PORT",
-	.states = v51_ctrl_port_fsm_states,
-	.num_states = ARRAY_SIZE(v51_ctrl_port_fsm_states),
-	.allstate_event_mask = 0,
-	.allstate_action = NULL,
-	.cleanup = NULL,
-	.timer_cb = v51_ctrl_port_fsm_timer_cb,
-	.log_subsys = DV5CTRL,
-	.event_names = v51_ctrl_port_fsm_event_names,
-};
+/* display MDU-error_indication */
+static void v51_mdu_error(struct osmo_fsm_inst *fi, const char *error)
+{
+	LOGP(DV5CTRL, LOGL_NOTICE, "MDU-error_indication: %s\n", error);
+}
 
-/***********************************************************************
- * common CTRL protocol FSM / G.964 Section 14
- ***********************************************************************/
+/* send control towards lower (DL) layer */
+static void v51_ctrl_send(struct v5x_ctrl_proto *ctrl, struct msgb *msg)
+{
+	struct osmo_fsm_inst *fi = ctrl->fi;
+
+	switch (fi->state) {
+	case V51_CTRL_ST_IN_SERVICE:
+		LOGP(DV5CTRL, LOGL_DEBUG, "We are in service, so we send our message now.\n");
+		/* no message, clone current message and store to be repeated */
+		ctrl->tx_msg = msgb_copy(msg, NULL);
+		/* go to AWAIT_PENDING_ACK */
+		osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_AWAIT_ACK, TIMEOUT, (ctrl->v5up) ? 1 : 2);
+		/* send message towards DL */
+#ifdef TEST_TX_FAILURE
+		msgb_free(msg);
+#else
+		v5x_snd((ctrl->v5if) ? : ctrl->v5up->inst, V51_DLADDR_CTRL, msg);
+#endif
+		break;
+	case V51_CTRL_ST_AWAIT_ACK:
+		LOGP(DV5CTRL, LOGL_DEBUG, "We are waiting for ack, so we queue our message.\n");
+		/* pending message, save message in queue */
+		msgb_enqueue(&ctrl->tx_queue, msg);
+		break;
+	}
+}
 
-enum v51_ctrl_common_fsm_state {
-	V51_CTRL_COM_ST_OUT_OF_SERVICE,
-	V51_CTRL_COM_ST_IN_SERVICE,
-	V51_CTRL_COM_AWAIT_COMMON_ACK,
-};
+/* receive acknowledge */
+static void v51_ctrl_ack(struct osmo_fsm_inst *fi, const struct tlv_parsed *tp)
+{
+	struct v5x_ctrl_proto *ctrl = fi->priv;
+	struct msgb *msg;
+
+	LOGP(DV5CTRL, LOGL_DEBUG, "Received acknowledge, removing pending message, if any.\n");
+	/* free pending copy of message, if acked before retry */
+	if (ctrl->tx_msg) {
+		msgb_free(ctrl->tx_msg);
+		ctrl->tx_msg = NULL;
+	}
+	/* go to IN_SERVICE */
+	osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_IN_SERVICE, 0, 0);
+	/* sending next pending message in queue */
+	msg = msgb_dequeue(&ctrl->tx_queue);
+	if (msg) {
+		LOGP(DV5CTRL, LOGL_DEBUG, "Found pending message in queue.\n");
+		v51_ctrl_send(ctrl, msg);
+	}
+}
 
-enum v51_ctrl_common_fsm_event {
-	V51_CTRL_CE_MDU_START_TRAFFIC,
-	V51_CTRL_CE_MDU_STOP_TRAFFIC,
-	V51_CTRL_CE_MDU_CTRL,
-	V51_CTRL_CE_RX_COMMON_CONTROL,
-	V51_CTRL_CE_RX_COMMON_CONTROL_ACK,
-};
+/* stop traffic */
+static void v51_ctrl_stop(struct osmo_fsm_inst *fi)
+{
+	struct v5x_ctrl_proto *ctrl = fi->priv;
+	struct msgb *msg;
 
-static const struct value_string v51_ctrl_common_fsm_event_names[] = {
-	{ V51_CTRL_CE_MDU_START_TRAFFIC,	"MDU-start_traffic" },
-	{ V51_CTRL_CE_MDU_STOP_TRAFFIC,		"MDU-stop_traffic" },
-	{ V51_CTRL_CE_MDU_CTRL,			"MDU-CTRL" },
-	{ V51_CTRL_CE_RX_COMMON_CONTROL,	"COMMON_CONTROL" },
-	{ V51_CTRL_CE_RX_COMMON_CONTROL_ACK,	"COMMON_CONTROL_ACK" },
-	{ 0, NULL }
-};
+	/* flush pending messages */
+	if (ctrl->tx_msg) {
+		msgb_free(ctrl->tx_msg);
+		ctrl->tx_msg = NULL;
+	}
+	while ((msg = msgb_dequeue(&ctrl->tx_queue)))
+		msgb_free(msg);
+	/* go to OUT_OF_SERVICE */
+	osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_OUT_OF_SERVICE, 0, 0);
+}
 
-static int v51_ctrl_common_fsm_timer_cb(struct osmo_fsm_inst *fi)
+/* T01 / T02 */
+static int v51_ctrl_fsm_timer_cb(struct osmo_fsm_inst *fi)
 {
-	if () {
-		/* first expiry: repeat COMMON_CONTROL; re-start T02 */
+	struct v5x_ctrl_proto *ctrl = fi->priv;
+	struct msgb *msg;
+
+	if (ctrl->tx_msg) {
+		LOGP(DV5CTRL, LOGL_DEBUG, "Timer fired the first time, resending message.\n");
+		/* first expiry: repeat CONTROL; re-start T01 */
+		/* send message towards DL */
+		osmo_timer_schedule(&fi->timer, TIMEOUT, 0);
+		msg = ctrl->tx_msg;
+		ctrl->tx_msg = NULL;
+		v5x_snd((ctrl->v5if) ? : ctrl->v5up->inst, V51_DLADDR_CTRL, msg);
 	} else {
+		LOGP(DV5CTRL, LOGL_DEBUG, "Timer fired the second time, indicate an error.\n");
 		/* second expiry: send MDU-error_ind; go to IN_SERVICE */
-		osmo_fsm_inst_state_chg(fi, V51_CTRL_COM_ST_IN_SERVICE);
+		v51_mdu_error(fi, "Second timeout while waiting for CONTROL ACK.");
+		osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_IN_SERVICE, 0, 0);
+		/* sending next pending message in queue */
+		msg = msgb_dequeue(&ctrl->tx_queue);
+		if (msg) {
+			LOGP(DV5CTRL, LOGL_DEBUG, "Found pending message in queue.\n");
+			v51_ctrl_send(ctrl, msg);
+		}
 	}
+
+	return 0;
 }
 
-static const struct osmo_fsm_state v51_ctrl_common_fsm_states[] = {
-	[V51_CTRL_COM_ST_OUT_OF_SERVICE] = {
+/* events in state OUT OF SERVICE */
+static void v51_ctrl_fsm_oos(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	switch (event) {
+		case V51_CTRL_EV_MDU_START_TRAFFIC:
+			/* go to IN_SERVICE */
+			osmo_fsm_inst_state_chg(fi, V51_CTRL_ST_IN_SERVICE, 0, 0);
+			break;
+		case V51_CTRL_EV_MDU_STOP_TRAFFIC:
+			v51_mdu_error(fi, "Got MDU-stop, but traffic not started.");
+			break;
+		case V51_CTRL_EV_MDU_CTRL:
+			msgb_free(data);
+			v51_mdu_error(fi, "Got MDU-CTRL, but traffic not started.");
+			break;
+		case V51_CTRL_EV_RX_CONTROL:
+			v51_mdu_error(fi, "Received CONTROL, but traffic not started.");
+			break;
+		case V51_CTRL_EV_RX_CONTROL_ACK:
+			v51_mdu_error(fi, "Received CONTROL ACK, but traffic not started.");
+			break;
+	}
+}
+
+/* events in state IN SERVICE */
+static void v51_ctrl_fsm_ins(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	switch (event) {
+		case V51_CTRL_EV_MDU_START_TRAFFIC:
+			break;
+		case V51_CTRL_EV_MDU_STOP_TRAFFIC:
+			v51_ctrl_stop(fi);
+			break;
+		case V51_CTRL_EV_MDU_CTRL:
+			v51_ctrl_send(fi->priv, data);
+			break;
+		case V51_CTRL_EV_RX_CONTROL:
+			v51_mdu_ctrl(fi, data);
+			break;
+	}
+}
+
+/* events in state AWAIT PORT/COMMON ACK */
+static void v51_ctrl_fsm_aa(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	switch (event) {
+		case V51_CTRL_EV_MDU_START_TRAFFIC:
+			break;
+		case V51_CTRL_EV_MDU_STOP_TRAFFIC:
+			v51_ctrl_stop(fi);
+			break;
+		case V51_CTRL_EV_MDU_CTRL:
+			v51_ctrl_send(fi->priv, data);
+			break;
+		case V51_CTRL_EV_RX_CONTROL:
+			v51_mdu_ctrl(fi, data);
+			break;
+		case V51_CTRL_EV_RX_CONTROL_ACK:
+			v51_ctrl_ack(fi, data);
+			break;
+	}
+}
+
+static const struct osmo_fsm_state v51_ctrl_fsm_states[] = {
+	[V51_CTRL_ST_OUT_OF_SERVICE] = {
 		.name = "OUT_OF_SERVICE",
-		.in_event_mask = S(V51_CTRL_CE_MDU_START_TRAFFIC) |
-				 S(V51_CTRL_CE_MDU_STOP_TRAFFIC) |
-				 S(V51_CTRL_CE_MDU_CTRL) |
-				 S(V51_CTRL_CE_RX_COMMON_CONTROL) |
-				 S(V51_CTRL_CE_RX_COMMON_CONTROL_ACK),
-		.out_state_mask = S(V51_CTRL_COM_ST_IN_SERVICE),
+		.in_event_mask = S(V51_CTRL_EV_MDU_START_TRAFFIC) |
+				 S(V51_CTRL_EV_MDU_STOP_TRAFFIC) |
+				 S(V51_CTRL_EV_MDU_CTRL) |
+				 S(V51_CTRL_EV_RX_CONTROL) |
+				 S(V51_CTRL_EV_RX_CONTROL_ACK),
+		.out_state_mask = S(V51_CTRL_ST_IN_SERVICE),
+		.action = v51_ctrl_fsm_oos,
 	},
-	[V51_CTRL_COM_ST_IN_SERVICE] = {
+	[V51_CTRL_ST_IN_SERVICE] = {
 		.name = "IN_SERVICE",
-		.in_event_mask = S(V51_CTRL_CE_MDU_START_TRAFFIC) |
-				 S(V51_CTRL_CE_MDU_STOP_TRAFFIC) |
-				 S(V51_CTRL_CE_MDU_CTRL) |
-				 S(V51_CTRL_CE_RX_COMMON_CONTROL),
-		.out_state_mask = S(V51_CTRL_COM_ST_OUT_OF_SERVICE) |
-				  S(V51_CTRL_COM_AWAIT_COMMON_ACK),
+		.in_event_mask = S(V51_CTRL_EV_MDU_START_TRAFFIC) |
+				 S(V51_CTRL_EV_MDU_STOP_TRAFFIC) |
+				 S(V51_CTRL_EV_MDU_CTRL) |
+				 S(V51_CTRL_EV_RX_CONTROL),
+
+		.out_state_mask = S(V51_CTRL_ST_OUT_OF_SERVICE) |
+				  S(V51_CTRL_ST_AWAIT_ACK),
+		.action = v51_ctrl_fsm_ins,
 	},
-	[V51_CTRL_COM_AWAIT_COMMON_ACK] = {
-		.name = "AWAIT_COMMON_ACK",
-		.in_event_mask = S(V51_CTRL_CE_MDU_START_TRAFFIC) |
-				 S(V51_CTRL_CE_MDU_STOP_TRAFFIC) |
-				 S(V51_CTRL_CE_MDU_CTRL) |
-				 S(V51_CTRL_CE_RX_COMMON_CONTROL) |
-				 S(V51_CTRL_CE_RX_COMMON_CONTROL_ACK),
-		.out_state_mask = S(V51_CTRL_COM_ST_OUT_OF_SERVICE) |
-				  S(V51_CTRL_COM_ST_IN_SERVICE),
+	[V51_CTRL_ST_AWAIT_ACK] = {
+		.name = "AWAIT_ACK",
+		.in_event_mask = S(V51_CTRL_EV_MDU_START_TRAFFIC) |
+				 S(V51_CTRL_EV_MDU_STOP_TRAFFIC) |
+				 S(V51_CTRL_EV_MDU_CTRL) |
+				 S(V51_CTRL_EV_RX_CONTROL) |
+				 S(V51_CTRL_EV_RX_CONTROL_ACK),
+		.out_state_mask = S(V51_CTRL_ST_OUT_OF_SERVICE) |
+				  S(V51_CTRL_ST_IN_SERVICE),
+		.action = v51_ctrl_fsm_aa,
 	},
 };
 
-static struct osmo_fsm v51_ctrl_common_fsm = {
-	.name = "V51_CTRL_COMMON",
-	.states = v51_ctrl_common_fsm_states,
-	.num_states = ARRAY_SIZE(v51_ctrl_common_fsm_states),
+struct osmo_fsm v51_ctrl_fsm = {
+	.name = "V51_CTRL",
+	.states = v51_ctrl_fsm_states,
+	.num_states = ARRAY_SIZE(v51_ctrl_fsm_states),
 	.allstate_event_mask = 0,
 	.allstate_action = NULL,
 	.cleanup = NULL,
-	.timer_cb = v51_ctrl_common_fsm_timer_cb,
+	.timer_cb = v51_ctrl_fsm_timer_cb,
 	.log_subsys = DV5CTRL,
-	.event_names = v51_ctrl_common_fsm_event_names,
+	.event_names = v51_ctrl_fsm_event_names,
 };
 
+struct v5x_ctrl_proto *v51_ctrl_create(struct v5x_interface *v5if, struct v5x_user_port *v5up, uint16_t nr)
+{
+	struct v5x_ctrl_proto *ctrl;
+
+	assert((v5if && !v5up) || (!v5if && v5up));
+
+	ctrl = talloc_zero((v5if) ? : v5up->inst, struct v5x_ctrl_proto);
+	if (!ctrl)
+		return NULL;
+	ctrl->v5if = v5if;
+	ctrl->v5up = v5up;
+	INIT_LLIST_HEAD(&ctrl->tx_queue);
+
+	ctrl->fi = osmo_fsm_inst_alloc(&v51_ctrl_fsm, ctrl, ctrl, LOGL_DEBUG, NULL);
+	if (!ctrl->fi)
+		return NULL;
+	osmo_fsm_inst_update_id_f(ctrl->fi, "%d", nr);
+
+	return ctrl;
+}
+
 
 /***********************************************************************
  * V5 Message encoding / sending
  ***********************************************************************/
 
 /* G.964 Section 14.4.1.1 / Table 48 */
-struct mgsb *v51_enc_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe)
+struct msgb *v51_enc_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe)
 {
-	uint8_t cfe_ie = cfe;
+	uint8_t cfe_ie = cfe | 0x80;
 	struct v51_l3_hdr *l3h;
 	struct msgb *msg = msgb_alloc_v5x();
 	if (!msg)
@@ -218,7 +371,7 @@ struct mgsb *v51_enc_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el
 
 	l3h = (struct v51_l3_hdr *) msgb_put(msg, sizeof(*l3h));
 	l3h->pdisc = V51_CTRL_PDISC;
-	l3h->l3_addr = v51_l3_addr_enc(v5up->nr, true);
+	l3h->l3_addr = ntohs(v51_l3_addr_enc(v5up->nr, true));
 	l3h->msg_type = V51_CTRL_MSGT_PORT_CTRL;
 
 	msgb_tlv_put(msg, V51_CTRL_IEI_CTRL_F_ELEMENT, 1, &cfe_ie);
@@ -229,7 +382,7 @@ struct mgsb *v51_enc_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el
 /* G.964 Section 14.4.1.2 / Table 49 */
 static struct msgb *v51_enc_ctrl_port_ack(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe)
 {
-	uint8_t cfe_ie = cfe;
+	uint8_t cfe_ie = cfe | 0x80;
 	struct v51_l3_hdr *l3h;
 	struct msgb *msg = msgb_alloc_v5x();
 	if (!msg)
@@ -237,7 +390,7 @@ static struct msgb *v51_enc_ctrl_port_ack(struct v5x_user_port *v5up, enum v51_c
 
 	l3h = (struct v51_l3_hdr *) msgb_put(msg, sizeof(*l3h));
 	l3h->pdisc = V51_CTRL_PDISC;
-	l3h->l3_addr = v51_l3_addr_enc(v5up->nr, true);
+	l3h->l3_addr = ntohs(v51_l3_addr_enc(v5up->nr, true));
 	l3h->msg_type = V51_CTRL_MSGT_PORT_CTRL_ACK;
 
 	msgb_tlv_put(msg, V51_CTRL_IEI_CTRL_F_ELEMENT, 1, &cfe_ie);
@@ -246,10 +399,10 @@ static struct msgb *v51_enc_ctrl_port_ack(struct v5x_user_port *v5up, enum v51_c
 }
 
 /* G.964 Section 14.4.1.3 / Table 50 */
-static struct msgb *v51_enc_ctrl_common(struct v5x_instance *v5i, enum v51_ctrl_func_id cfi,
+static struct msgb *v51_enc_ctrl_common(struct v5x_interface *v5if, enum v51_ctrl_func_id cfi,
 					uint8_t *rej_cause, uint8_t *variant, uint32_t *interface_id)
 {
-	uint8_t cfi_ie = cfi;
+	uint8_t cfi_ie = cfi | 0x80;
 	struct v51_l3_hdr *l3h;
 	struct msgb *msg = msgb_alloc_v5x();
 	if (!msg)
@@ -257,17 +410,18 @@ static struct msgb *v51_enc_ctrl_common(struct v5x_instance *v5i, enum v51_ctrl_
 
 	l3h = (struct v51_l3_hdr *) msgb_put(msg, sizeof(*l3h));
 	l3h->pdisc = V51_CTRL_PDISC;
-	l3h->l3_addr = v51_l3_addr_enc(V51_DLADDR_CTRL, true);
+	l3h->l3_addr = ntohs(v51_l3_addr_enc(V51_DLADDR_CTRL, true));
 	l3h->msg_type = V51_CTRL_MSGT_COMMON_CTRL;
 
 	msgb_tlv_put(msg, V51_CTRL_IEI_CTRL_F_ID, 1, &cfi_ie);
 	if (variant) {
 		/* Conditional: Variant */
-		msgb_tlv_put(msg, V51_CTRL_IEI_VARIANT, 1, variant);
+		uint8_t variant_ie = *variant | 0x80;
+		msgb_tlv_put(msg, V51_CTRL_IEI_VARIANT, 1, &variant_ie);
 	}
 	if (rej_cause) {
 		/* Conditional: Rejection Cause */
-		msgb_put_u8(0xF0 | (*rej_cause & 0x0F));
+		msgb_put_u8(msg, 0xF0 | (*rej_cause & 0x0F));
 	}
 	if (interface_id) {
 		/* Conditional: Interface Id */
@@ -280,9 +434,9 @@ static struct msgb *v51_enc_ctrl_common(struct v5x_instance *v5i, enum v51_ctrl_
 }
 
 /* G.964 Section 14.4.1.4 / Table 51 */
-static struct msgb *v51_enc_ctrl_common_ack(struct v5x_instance *v5i, enum v51_ctrl_func_id cfi)
+static struct msgb *v51_enc_ctrl_common_ack(struct v5x_interface *v5if, enum v51_ctrl_func_id cfi)
 {
-	uint8_t cfi_ie = cfi;
+	uint8_t cfi_ie = cfi | 0x80;
 	struct v51_l3_hdr *l3h;
 	struct msgb *msg = msgb_alloc_v5x();
 	if (!msg)
@@ -290,7 +444,7 @@ static struct msgb *v51_enc_ctrl_common_ack(struct v5x_instance *v5i, enum v51_c
 
 	l3h = (struct v51_l3_hdr *) msgb_put(msg, sizeof(*l3h));
 	l3h->pdisc = V51_CTRL_PDISC;
-	l3h->l3_addr = v51_l3_addr_enc(V51_DLADDR_CTRL, true);
+	l3h->l3_addr = htons(v51_l3_addr_enc(V51_DLADDR_CTRL, true));
 	l3h->msg_type = V51_CTRL_MSGT_COMMON_CTRL_ACK;
 
 	msgb_tlv_put(msg, V51_CTRL_IEI_CTRL_F_ID, 1, &cfi_ie);
@@ -302,70 +456,89 @@ static struct msgb *v51_enc_ctrl_common_ack(struct v5x_instance *v5i, enum v51_c
  * V5 Message receiving / decoding
  ***********************************************************************/
 
-static int v51_rcv_ctrl_port(struct v5x_user_port *v5up, struct msgb *msg, const struct tlv_parsed *tp)
+static int v51_rcv_ctrl_port(struct v5x_user_port *v5up, uint8_t msg_type, const struct tlv_parsed *tp)
 {
-	uint16_t l3_addr;
-	enum v51_ctrl_func_el cfe = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ELEMENT);
-	struct v5x_user_port *port = FIXME;
+	enum v51_ctrl_func_el cfe = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ELEMENT) & 0x7f;
 
-	switch (l3h->msg_type) {
+	switch (msg_type) {
 	case V51_CTRL_MSGT_PORT_CTRL:
 		/* FIXME: send event to FSM */
-		osmo_fsm_inst_dispatch(port->ctrl_fi, V51_CTRL_PE_RX_PORT_CONTROL, tp);
+		osmo_fsm_inst_dispatch(v5up->ctrl->fi, V51_CTRL_EV_RX_CONTROL, (void *)tp);
 		/* send ACK to AN */
-		return v51_tx(v5up->inst, v51_enc_ctrl_port_ack(v5up, cfe));
+		return v5x_snd(v5up->inst, V51_DLADDR_CTRL, v51_enc_ctrl_port_ack(v5up, cfe));
 	case V51_CTRL_MSGT_PORT_CTRL_ACK:
-		osmo_fsm_inst_dispatch(port->ctrl_fi, V51_CTRL_PE_RX_PORT_CONTROL_ACK, tp);
+		osmo_fsm_inst_dispatch(v5up->ctrl->fi, V51_CTRL_EV_RX_CONTROL_ACK, (void *)tp);
 	default:
 		return -EINVAL;
 	}
 }
 
-static int v51_rcv_ctrl_common(struct v5x_instance *v5i, struct msgb *msg, const struct tlv_parsed *tp)
+static int v51_rcv_ctrl_common(struct v5x_interface *v5if, uint8_t msg_type, const struct tlv_parsed *tp)
 {
-	uint16_t l3_addr;
-	enum v51_ctrl_func_id cfi = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ID);
+	enum v51_ctrl_func_id cfi = *TLVP_VAL(tp, V51_CTRL_IEI_CTRL_F_ID) & 0x7f;
 
-	switch (l3h->msg_type) {
+	switch (msg_type) {
 	case V51_CTRL_MSGT_COMMON_CTRL:
 		/* send event to FSM */
-		osmo_fsm_inst_dispatch(v5i->ctrl_fi, V51_CTRL_CE_RX_COMMON_CONTROL, tp);
+		osmo_fsm_inst_dispatch(v5if->control.ctrl->fi, V51_CTRL_EV_RX_CONTROL, (void *)tp);
 		/* send ACK to AN */
-		return v51_tx(v5up->inst, v51_enc_ctrl_common_ack(v5i, cfi));
+		printf("sending ack\n");
+		return v5x_snd(v5if, V51_DLADDR_CTRL, v51_enc_ctrl_common_ack(v5if, cfi));
 	case V51_CTRL_MSGT_COMMON_CTRL_ACK:
 		/* send event to FSM */
-		osmo_fsm_inst_dispatch(v5i->ctrl_fi, V51_CTRL_CE_RX_COMMON_CONTROL_ACK, tp);
-		break;
+		osmo_fsm_inst_dispatch(v5if->control.ctrl->fi, V51_CTRL_EV_RX_CONTROL_ACK, (void *)tp);
+		return 0;
 	default:
 		return -EINVAL;
 	}
 }
 
-
-static int v51_rcv_ctrl(struct v5x_instance *v5i, struct msgb *msg, const struct tlv_parsed *tp)
+/* receive message from lower (DL) layer */
+int v51_rcv_ctrl(struct v5x_interface *v5if, uint16_t l3_addr, uint8_t msg_type, const struct tlv_parsed *tp)
 {
-	struct v51_l3_hdr *l3h = msgb_l3(msg);
-	uint16_t l3_addr;
-	bool is_isdn;
-	int rc;
-
-	l3_addr = v51_l3_addr_dec(l3h->l3_addr, &is_isdn);
-	if (!is_isdn)
-		return -EINVAL;
+	struct v5x_user_port *v5up;
 
-	switch (l3h->msg_type) {
+	switch (msg_type) {
 	case V51_CTRL_MSGT_PORT_CTRL:
 	case V51_CTRL_MSGT_PORT_CTRL_ACK:
-		v5up = v5x_user_port_find(v5i, l4_addr);
+		v5up = v5x_user_port_find(v5if, l3_addr);
 		if (!v5up)
 			return -ENODEV;
-		return v5x_rcv_ctrl_port(v5up, msg, tp);
+		return v51_rcv_ctrl_port(v5up, msg_type, tp);
 	case V51_CTRL_MSGT_COMMON_CTRL:
 	case V51_CTRL_MSGT_COMMON_CTRL_ACK:
 		if (l3_addr != V51_DLADDR_CTRL)
 			return -EINVAL;
-		return v5x_rcv_ctrl_common(v5i, msg, tp);
+		return v51_rcv_ctrl_common(v5if, msg_type, tp);
 	}
 
-	return rc;
+	return -EINVAL;
+}
+
+/***********************************************************************
+ * V5 Message sending / encoding
+ ***********************************************************************/
+
+/* send common message from upper layer */
+int v51_snd_ctrl_common(struct v5x_interface *v5if, enum v51_ctrl_func_id cfi,
+			uint8_t *rej_cause, uint8_t *variant, uint32_t *interface_id)
+{
+	osmo_fsm_inst_dispatch(v5if->control.ctrl->fi, V51_CTRL_EV_MDU_CTRL, v51_enc_ctrl_common(v5if, cfi, rej_cause, variant, interface_id));
+	return 0;
+}
+
+/* send port message from upper layer */
+int v51_snd_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe)
+{
+	osmo_fsm_inst_dispatch(v5up->ctrl->fi, V51_CTRL_EV_MDU_CTRL, v51_enc_ctrl_port(v5up, cfe));
+	return 0;
+}
+
+void v51_start_ctrl(struct v5x_ctrl_proto *ctrl)
+{
+	osmo_fsm_inst_dispatch(ctrl->fi, V51_CTRL_EV_MDU_START_TRAFFIC, NULL);
+}
+void v51_stop_ctrl(struct v5x_ctrl_proto *ctrl)
+{
+	osmo_fsm_inst_dispatch(ctrl->fi, V51_CTRL_EV_MDU_START_TRAFFIC, NULL);
 }

v51_le_ctrl.h

@@ -0,0 +1,10 @@
+
+extern struct osmo_fsm v51_ctrl_fsm;
+
+struct v5x_ctrl_proto *v51_ctrl_create(struct v5x_interface *v5if, struct v5x_user_port *v5up, uint16_t nr);
+int v51_rcv_ctrl(struct v5x_interface *v5if, uint16_t l3_addr, uint8_t msg_type, const struct tlv_parsed *tp);
+int v51_snd_ctrl_common(struct v5x_interface *v5if, enum v51_ctrl_func_id cfi,
+			uint8_t *rej_cause, uint8_t *variant, uint32_t *interface_id);
+int v51_snd_ctrl_port(struct v5x_user_port *v5up, enum v51_ctrl_func_el cfe);
+void v51_start_ctrl(struct v5x_ctrl_proto *ctrl);
+void v51_stop_ctrl(struct v5x_ctrl_proto *ctrl);

v5x_data.c

@@ -2,6 +2,9 @@
 #include <osmocom/core/linuxlist.h>
 
 #include "v5x_internal.h"
+#include "v5x_protocol.h"
+#include "v51_le_ctrl.h"
+#include "lapv5.h"
 
 static LLIST_HEAD(v5_instances);
 
@@ -12,8 +15,6 @@ struct v5x_instance *v5x_instance_alloc(void *ctx)
 		return NULL;
 
 	INIT_LLIST_HEAD(&v5i->interfaces);
-	INIT_LLIST_HEAD(&v5i->user_ports);
-	/* TODO: allocate ctrl_fi */
 
 	llist_add_tail(&v5i->list, &v5_instances);
 
@@ -49,7 +50,8 @@ static void v5x_link_init(struct v5x_link *v5l, struct v5x_interface *v5if, uint
 	v5x_cchan_init(&v5l->c_channel[0], v5l, &v5l->ts[16], true);
 }
 
-struct v5x_interface *v5x_interface_alloc(struct v5x_instance *v5i, enum v5x_dialect dialect)
+struct v5x_interface *v5x_interface_alloc(struct v5x_instance *v5i, enum v5x_dialect dialect,
+	uint32_t id, uint8_t variant, int (*ph_data_req_cb)(struct msgb *msg, void *cbdata))
 {
 	struct v5x_interface *v5if = talloc_zero(v5i, struct v5x_interface);
 	//struct v5x_link *v5l;
@@ -57,6 +59,8 @@ struct v5x_interface *v5x_interface_alloc(struct v5x_instance *v5i, enum v5x_dia
 
 	v5if->instance = v5i;
 	v5if->dialect = dialect;
+	v5if->id = id;
+	v5if->variant = variant;
 
 	/* primary link must alwasy be present */
 	v5x_link_init(&v5if->links[0], v5if, 0);
@@ -64,29 +68,77 @@ struct v5x_interface *v5x_interface_alloc(struct v5x_instance *v5i, enum v5x_dia
 
 	primary_c_chan = &v5if->primary_link->c_channel[0];
 
-	
-	/* TODO: allocate fi */
-
-	//lapd_dl_init2(&v5if->control.dl, k, v_rnage, maxf, "control");
+	v5if->control.ctrl = v51_ctrl_create(v5if, NULL, 0);
+	if (!v5if->control.ctrl)
+		return NULL;
+	v5if->control.li = lapv5_instance_alloc(1, ph_data_req_cb, v5if, v5x_rcv, v5if, &lapd_profile_lapv5dl, "control");
 	v5if->control.c_chan = primary_c_chan;
 
-	//lapd_dl_init2(&v5if->pstn.dl, k, v_rnage, maxf, "pstn");
+	v5if->pstn.li = lapv5_instance_alloc(1, ph_data_req_cb, v5if, v5x_rcv, v5if, &lapd_profile_lapv5dl, "pstn");
 	v5if->pstn.c_chan = primary_c_chan;
 
 	if (v5if->dialect == V5X_DIALECT_V52) {
-		//lapd_dl_init2(&v5if->lcp.dl, k, v_rnage, maxf, "lcp");
+		v5if->lcp.li = lapv5_instance_alloc(1, ph_data_req_cb, v5if, v5x_rcv, v5if, &lapd_profile_lapv5dl, "lcp");
 		v5if->lcp.c_chan = primary_c_chan;
 
-		//lapd_dl_init2(&v5if->bcc.dl, k, v_rnage, maxf, "bcc");
+		v5if->bcc.li = lapv5_instance_alloc(1, ph_data_req_cb, v5if, v5x_rcv, v5if, &lapd_profile_lapv5dl, "bcc");
 		v5if->bcc.c_chan = primary_c_chan;
 
-		//lapd_dl_init2(&v5if->protection[0].dl, k, v_rnage, maxf, "protection0");
+		v5if->protection[0].li = lapv5_instance_alloc(1, ph_data_req_cb, v5if, v5x_rcv, v5if, &lapd_profile_lapv5dl, "protection0");
 		v5if->protection[0].c_chan = primary_c_chan;
 
 		//protection[1] ? 
 	}
 
+	INIT_LLIST_HEAD(&v5if->user_ports);
+
 	llist_add_tail(&v5if->list, &v5i->interfaces);
 
 	return v5if;
 }
+
+struct v5x_user_port *v5x_user_port_create(struct v5x_interface *v5if, uint16_t nr, bool is_isdn)
+{
+	struct v5x_user_port *v5up;
+
+	v5up = talloc_zero(v5if, struct v5x_user_port);
+	if (!v5up)
+		return NULL;
+	v5up->inst = v5if;
+	v5up->is_isdn = is_isdn;
+	v5up->ctrl = v51_ctrl_create(NULL, v5up, nr);
+	if (!v5up->ctrl)
+		return NULL;
+	if (is_isdn) {
+		/* TODO: allocate fi
+		v5up->state = v52_isdn_state_create(v5up, nr);
+		if (!v5up->state)
+			return NULL;
+		*/
+	} else {
+		/* TODO: allocate fi
+		v5up->state = v52_pstn_state_create(v5up, nr);
+		if (!v5up->state)
+			return NULL;
+		v5up->pstn.proto = v52_pstn_protocol_create(v5up, nr);
+		if (!v5up->pstn.proto)
+			return NULL;
+		*/
+	}
+	v5up->nr = nr;
+
+	llist_add_tail(&v5up->list, &v5if->user_ports);
+
+	return v5up;
+}
+
+struct v5x_user_port *v5x_user_port_find(struct v5x_interface *v5if, uint16_t nr)
+{
+	struct v5x_user_port *v5up;
+
+	llist_for_each_entry(v5up, &v5if->user_ports, list) {
+		if (v5up->nr == nr)
+			return v5up;
+	}
+	return NULL;
+}

v5x_internal.h

@@ -25,6 +25,7 @@
 
 #include <stdint.h>
 #include <osmocom/core/utils.h>
+#include <osmocom/core/fsm.h>
 #include <osmocom/gsm/lapd_core.h>
 
 struct osmo_fsm_inst;
@@ -37,6 +38,7 @@ enum v5x_dialect {
 /* forward-declarations */
 struct v5x_interface;
 struct v5x_instance;
+struct v5x_user_port;
 struct v5x_link;
 
 /* A C-channel is a 64k timeslot used for signalling */