host/trxcon: initial release of L1CTL handlers

Now it's possible to handle the following requests
from layer23 apps:

- L1CTL_FBSB_REQ
- L1CTL_PM_REQ
- L1CTL_RESET_REQ
- L1CTL_ECHO_REQ

It should be noted, that the L1CTL_PM_REQ isn't
handled correctly yet, due to required task isn't
implemented on the TRX side yet. Instead of this,
temporary we are sending some fake responses.

Change-Id: I343eca3e20922ddd83e06231811200b26da442f3

7 files changed, 290 insertions, 2 deletions

diff --git a/src/host/trxcon/Makefile.am b/src/host/trxcon/Makefile.am
index 9da21996..869ed8ba 100644
--- a/src/host/trxcon/Makefile.am
+++ b/src/host/trxcon/Makefile.am
@@ -22,6 +22,7 @@ bin_PROGRAMS = trxcon
 
 trxcon_SOURCES = \
 	l1ctl_link.c \
+	l1ctl.c \
 	trx_if.c \
 	logging.c \
 	trxcon.c \
diff --git a/src/host/trxcon/l1ctl.c b/src/host/trxcon/l1ctl.c
new file mode 100644
index 00000000..26670f1f
--- /dev/null
+++ b/src/host/trxcon/l1ctl.c
@@ -0,0 +1,253 @@
+/*
+ * OsmocomBB <-> SDR connection bridge
+ * GSM L1 control interface handlers
+ *
+ * (C) 2014 by Sylvain Munaut <tnt@246tNt.com>
+ * (C) 2016-2017 by Vadim Yanitskiy <axilirator@gmail.com>
+ *
+ * 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.
+ *
+ */
+
+#include <stdio.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <assert.h>
+
+#include <arpa/inet.h>
+
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/select.h>
+#include <osmocom/gsm/gsm_utils.h>
+
+#include "trxcon.h"
+#include "logging.h"
+#include "l1ctl_link.h"
+#include "l1ctl_proto.h"
+
+extern void *tall_trx_ctx;
+extern struct osmo_fsm_inst *trxcon_fsm;
+
+static struct msgb *l1ctl_alloc_msg(uint8_t msg_type)
+{
+	struct l1ctl_hdr *l1h;
+	struct msgb *msg = msgb_alloc_headroom(256, 4, "osmo_l1");
+
+	if (!msg) {
+		LOGP(DL1C, LOGL_ERROR, "Failed to allocate memory\n");
+		return NULL;
+	}
+
+	msg->l1h = msgb_put(msg, sizeof(*l1h));
+	l1h = (struct l1ctl_hdr *) msg->l1h;
+	l1h->msg_type = msg_type;
+
+	return msg;
+}
+
+int l1ctl_tx_pm_conf(struct l1ctl_link *l1l, uint16_t band_arfcn,
+	int dbm, int last)
+{
+	struct l1ctl_pm_conf *pmc;
+	struct msgb *msg;
+
+	msg = l1ctl_alloc_msg(L1CTL_PM_CONF);
+	if (!msg)
+		return -ENOMEM;
+
+	LOGP(DL1C, LOGL_DEBUG, "Send PM Conf (%s %d = %d dBm)\n",
+		gsm_band_name(gsm_arfcn2band(band_arfcn)),
+		band_arfcn &~ ARFCN_FLAG_MASK, dbm);
+
+	pmc = (struct l1ctl_pm_conf *) msgb_put(msg, sizeof(*pmc));
+	pmc->band_arfcn = htons(band_arfcn);
+	pmc->pm[0] = dbm2rxlev(dbm);
+	pmc->pm[1] = 0;
+
+	if (last) {
+		struct l1ctl_hdr *l1h = (struct l1ctl_hdr *) msg->l1h;
+		l1h->flags |= L1CTL_F_DONE;
+	}
+
+	return l1ctl_link_send(l1l, msg);
+}
+
+int l1ctl_tx_reset_ind(struct l1ctl_link *l1l, uint8_t type)
+{
+	struct msgb *msg;
+	struct l1ctl_reset *res;
+
+	msg = l1ctl_alloc_msg(L1CTL_RESET_IND);
+	if (!msg)
+		return -ENOMEM;
+
+	LOGP(DL1C, LOGL_DEBUG, "Send Reset Ind (%u)\n", type);
+
+	res = (struct l1ctl_reset *) msgb_put(msg, sizeof(*res));
+	res->type = type;
+
+	return l1ctl_link_send(l1l, msg);
+}
+
+int l1ctl_tx_reset_conf(struct l1ctl_link *l1l, uint8_t type)
+{
+	struct msgb *msg;
+	struct l1ctl_reset *res;
+
+	msg = l1ctl_alloc_msg(L1CTL_RESET_CONF);
+	if (!msg)
+		return -ENOMEM;
+
+	LOGP(DL1C, LOGL_DEBUG, "Send Reset Conf (%u)\n", type);
+	res = (struct l1ctl_reset *) msgb_put(msg, sizeof(*res));
+	res->type = type;
+
+	return l1ctl_link_send(l1l, msg);
+}
+
+static int l1ctl_rx_fbsb_req(struct l1ctl_link *l1l, struct msgb *msg)
+{
+	struct l1ctl_fbsb_req *fbsb;
+	uint16_t band_arfcn;
+	int rc = 0;
+
+	fbsb = (struct l1ctl_fbsb_req *) msg->l1h;
+	if (msgb_l1len(msg) < sizeof(*fbsb)) {
+		LOGP(DL1C, LOGL_ERROR, "MSG too short FBSB Req: %u\n",
+			msgb_l1len(msg));
+		rc = -EINVAL;
+		goto exit;
+	}
+
+	band_arfcn = ntohs(fbsb->band_arfcn);
+
+	LOGP(DL1C, LOGL_DEBUG, "Recv FBSB Req (%s %d)\n",
+		gsm_band_name(gsm_arfcn2band(band_arfcn)),
+		band_arfcn &~ ARFCN_FLAG_MASK);
+
+	osmo_fsm_inst_dispatch(trxcon_fsm,
+		L1CTL_EVENT_FBSB_REQ, &band_arfcn);
+
+exit:
+	msgb_free(msg);
+	return rc;
+}
+
+static int l1ctl_rx_pm_req(struct l1ctl_link *l1l, struct msgb *msg)
+{
+	uint16_t arfcn_start, arfcn_stop, arfcn;
+	struct l1ctl_pm_req *pmr;
+	int rc = 0;
+
+	pmr = (struct l1ctl_pm_req *) msg->l1h;
+	if (msgb_l1len(msg) < sizeof(*pmr)) {
+		LOGP(DL1C, LOGL_ERROR, "MSG too short PM Req: %u\n",
+			msgb_l1len(msg));
+		rc = -EINVAL;
+		goto exit;
+	}
+
+	arfcn_start = ntohs(pmr->range.band_arfcn_from);
+	arfcn_stop  = ntohs(pmr->range.band_arfcn_to);
+
+	LOGP(DL1C, LOGL_DEBUG, "Recv PM Req (%s: %d -> %d)\n",
+		gsm_band_name(gsm_arfcn2band(arfcn_start)),
+		arfcn_start &~ ARFCN_FLAG_MASK,
+		arfcn_stop &~ ARFCN_FLAG_MASK);
+
+	/**
+	 * HACK: power measurement isn't implemented yet,
+	 * sending fake results for now...
+	 *
+	 * FIXME: l1ctl_link.c:203 Failed to enqueue msg!
+	 * l1l->wq size is limited to 100, so we cannot
+	 * put more messages until osmo_select_main()
+	 * is called.
+	 */
+	for (arfcn = arfcn_start; arfcn <= arfcn_stop; arfcn++)
+		l1ctl_tx_pm_conf(l1l, arfcn, arfcn == 33 ?
+			-60 : -120, arfcn == arfcn_stop);
+
+exit:
+	msgb_free(msg);
+	return rc;
+}
+
+static int l1ctl_rx_reset_req(struct l1ctl_link *l1l, struct msgb *msg)
+{
+	struct l1ctl_reset *res;
+	int rc = 0;
+
+	res = (struct l1ctl_reset *) msg->l1h;
+	if (msgb_l1len(msg) < sizeof(*res)) {
+		LOGP(DL1C, LOGL_ERROR, "MSG too short Reset Req: %u\n",
+			msgb_l1len(msg));
+		rc = -EINVAL;
+		goto exit;
+	}
+
+	LOGP(DL1C, LOGL_DEBUG, "Recv Reset Req (%u)\n", res->type);
+
+	osmo_fsm_inst_dispatch(trxcon_fsm,
+		L1CTL_EVENT_RESET_REQ, res);
+
+exit:
+	msgb_free(msg);
+	return rc;
+}
+
+static int l1ctl_rx_echo_req(struct l1ctl_link *l1l, struct msgb *msg)
+{
+	struct l1ctl_hdr *l1h;
+
+	LOGP(DL1C, LOGL_NOTICE, "Recv Echo Req\n");
+	LOGP(DL1C, LOGL_NOTICE, "Send Echo Conf\n");
+
+	/* Nothing to do, just send it back */
+	l1h = (struct l1ctl_hdr *) msg->l1h;
+	l1h->msg_type = L1CTL_ECHO_CONF;
+	msg->data = msg->l1h;
+
+	return l1ctl_link_send(l1l, msg);
+}
+
+int l1ctl_rx_cb(struct l1ctl_link *l1l, struct msgb *msg)
+{
+	struct l1ctl_hdr *l1h;
+
+	l1h = (struct l1ctl_hdr *) msg->l1h;
+	msg->l1h = l1h->data;
+
+	switch (l1h->msg_type) {
+	case L1CTL_FBSB_REQ:
+		return l1ctl_rx_fbsb_req(l1l, msg);
+	case L1CTL_PM_REQ:
+		return l1ctl_rx_pm_req(l1l, msg);
+	case L1CTL_RESET_REQ:
+		return l1ctl_rx_reset_req(l1l, msg);
+	case L1CTL_ECHO_REQ:
+		return l1ctl_rx_echo_req(l1l, msg);
+	default:
+		LOGP(DL1C, LOGL_ERROR, "Unknown MSG: %u\n", l1h->msg_type);
+		msgb_free(msg);
+		return -EINVAL;
+	}
+}
diff --git a/src/host/trxcon/l1ctl.h b/src/host/trxcon/l1ctl.h
new file mode 100644
index 00000000..ffd1a81f
--- /dev/null
+++ b/src/host/trxcon/l1ctl.h
@@ -0,0 +1,12 @@
+#pragma once
+
+#include <stdint.h>
+#include <osmocom/core/msgb.h>
+
+#include "l1ctl_link.h"
+
+int l1ctl_tx_pm_conf(struct l1ctl_link *l1l, uint16_t band_arfcn,
+	int dbm, int last);
+int l1ctl_tx_reset_conf(struct l1ctl_link *l1l, uint8_t type);
+int l1ctl_tx_reset_ind(struct l1ctl_link *l1l, uint8_t type);
+int l1ctl_rx_cb(struct l1ctl_link *l1l, struct msgb *msg);
diff --git a/src/host/trxcon/l1ctl_link.c b/src/host/trxcon/l1ctl_link.c
index 34aa4aaf..d2ceb177 100644
--- a/src/host/trxcon/l1ctl_link.c
+++ b/src/host/trxcon/l1ctl_link.c
@@ -43,6 +43,7 @@
 #include "trxcon.h"
 #include "logging.h"
 #include "l1ctl_link.h"
+#include "l1ctl.h"
 
 extern void *tall_trx_ctx;
 extern struct osmo_fsm_inst *trxcon_fsm;
@@ -112,8 +113,8 @@ static int l1ctl_link_read_cb(struct osmo_fd *bfd)
 	LOGP(DL1C, LOGL_DEBUG, "RX: '%s'\n",
 		osmo_hexdump(msg->data, msg->len));
 
-	/* TODO: call L1CTL handler here */
-	msgb_free(msg);
+	/* Call L1CTL handler */
+	l1ctl_rx_cb(l1l, msg);
 
 	return 0;
 }
diff --git a/src/host/trxcon/l1ctl_proto.h b/src/host/trxcon/l1ctl_proto.h
new file mode 120000
index 00000000..75862bae
--- /dev/null
+++ b/src/host/trxcon/l1ctl_proto.h
@@ -0,0 +1 @@
+../../../include/l1ctl_proto.h
\ No newline at end of file
diff --git a/src/host/trxcon/trxcon.c b/src/host/trxcon/trxcon.c
index 5874560d..a90d038d 100644
--- a/src/host/trxcon/trxcon.c
+++ b/src/host/trxcon/trxcon.c
@@ -39,7 +39,9 @@
 #include "trxcon.h"
 #include "trx_if.h"
 #include "logging.h"
+#include "l1ctl.h"
 #include "l1ctl_link.h"
+#include "l1ctl_proto.h"
 
 #define COPYRIGHT \
 	"Copyright (C) 2016-2017 by Vadim Yanitskiy <axilirator@gmail.com>\n" \
@@ -76,6 +78,8 @@ static void trxcon_fsm_idle_action(struct osmo_fsm_inst *fi,
 static void trxcon_fsm_managed_action(struct osmo_fsm_inst *fi,
 	uint32_t event, void *data)
 {
+	uint16_t *band_arfcn;
+
 	switch (event) {
 	case L1CTL_EVENT_DISCONNECT:
 		osmo_fsm_inst_state_chg(trxcon_fsm, TRXCON_STATE_IDLE, 0, 0);
@@ -85,7 +89,19 @@ static void trxcon_fsm_managed_action(struct osmo_fsm_inst *fi,
 			trx_if_cmd_poweroff(app_data.trx);
 		}
 		break;
+	case L1CTL_EVENT_RESET_REQ:
+		trx_if_cmd_echo(app_data.trx);
+		break;
 	case TRX_EVENT_RESET_IND:
+		/* TODO: send proper reset type */
+		l1ctl_tx_reset_conf(app_data.l1l, L1CTL_RES_T_BOOT);
+		break;
+	case L1CTL_EVENT_FBSB_REQ:
+		band_arfcn = (uint16_t *) data;
+		trx_if_cmd_rxtune(app_data.trx, *band_arfcn);
+		trx_if_cmd_txtune(app_data.trx, *band_arfcn);
+		trx_if_cmd_poweron(app_data.trx);
+		break;
 	case TRX_EVENT_RSP_ERROR:
 	case TRX_EVENT_OFFLINE:
 		/* TODO: notify L2 & L3 about that */
@@ -105,6 +121,8 @@ static struct osmo_fsm_state trxcon_fsm_states[] = {
 	[TRXCON_STATE_MANAGED] = {
 		.in_event_mask = (
 			GEN_MASK(L1CTL_EVENT_DISCONNECT) |
+			GEN_MASK(L1CTL_EVENT_FBSB_REQ) |
+			GEN_MASK(L1CTL_EVENT_RESET_REQ) |
 			GEN_MASK(TRX_EVENT_RESET_IND) |
 			GEN_MASK(TRX_EVENT_RSP_ERROR) |
 			GEN_MASK(TRX_EVENT_OFFLINE)),
diff --git a/src/host/trxcon/trxcon.h b/src/host/trxcon/trxcon.h
index a7a3a65f..9535578e 100644
--- a/src/host/trxcon/trxcon.h
+++ b/src/host/trxcon/trxcon.h
@@ -11,6 +11,8 @@ enum trxcon_fsm_events {
 	/* L1CTL specific events */
 	L1CTL_EVENT_CONNECT,
 	L1CTL_EVENT_DISCONNECT,
+	L1CTL_EVENT_FBSB_REQ,
+	L1CTL_EVENT_RESET_REQ,
 
 	/* TRX specific events */
 	TRX_EVENT_RESET_IND,