sysmobts: Use the ctrl interface for calibration

This runs the entire procedure for calibration with reasonable
error and success checking. It can be triggered from the VTY
of the sysmobts-mgr right now.

What is missing is to hook up with GPSD to check if the system
has a fix and provide a mode that will continously run the
calibration command.

4 files changed, 261 insertions, 5 deletions

diff --git a/src/osmo-bts-sysmo/Makefile.am b/src/osmo-bts-sysmo/Makefile.am
index 2cad5c0f..0edaa27d 100644
--- a/src/osmo-bts-sysmo/Makefile.am
+++ b/src/osmo-bts-sysmo/Makefile.am
@@ -29,7 +29,7 @@ sysmobts_mgr_SOURCES = \
 		misc/sysmobts_mgr_temp.c \
 		misc/sysmobts_mgr_calib.c \
 		eeprom.c
-sysmobts_mgr_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOVTY_LIBS) $(LIBOSMOABIS_LIBS) $(LIBOSMOGSM_LIBS) $(top_builddir)/src/common/libbts.a
+sysmobts_mgr_LDADD = $(LIBOSMOCORE_LIBS) $(LIBOSMOVTY_LIBS) $(LIBOSMOABIS_LIBS) $(LIBOSMOGSM_LIBS) $(LIBOSMOCTRL_LIBS) $(top_builddir)/src/common/libbts.a
 
 sysmobts_util_SOURCES = misc/sysmobts_util.c misc/sysmobts_par.c eeprom.c
 sysmobts_util_LDADD = $(LIBOSMOCORE_LIBS)
diff --git a/src/osmo-bts-sysmo/misc/sysmobts_mgr.h b/src/osmo-bts-sysmo/misc/sysmobts_mgr.h
index 21b442e9..85617304 100644
--- a/src/osmo-bts-sysmo/misc/sysmobts_mgr.h
+++ b/src/osmo-bts-sysmo/misc/sysmobts_mgr.h
@@ -6,6 +6,8 @@
 
 #include <osmocom/core/timer.h>
 
+#include <stdint.h>
+
 enum {
 	DTEMP,
 	DFW,
@@ -82,6 +84,10 @@ struct sysmobts_mgr_instance {
 		int is_up;
 		struct osmo_timer_list recon_timer;
 		struct ipa_client_conn *bts_conn;
+
+		int state;
+		struct osmo_timer_list timer;
+		uint32_t last_seqno;
 	} calib;
 };
 
@@ -89,10 +95,13 @@ int sysmobts_mgr_vty_init(void);
 int sysmobts_mgr_parse_config(struct sysmobts_mgr_instance *mgr);
 int sysmobts_mgr_nl_init(void);
 int sysmobts_mgr_temp_init(struct sysmobts_mgr_instance *mgr);
-int sysmobts_mgr_calib_init(struct sysmobts_mgr_instance *mgr);
 const char *sysmobts_mgr_temp_get_state(enum sysmobts_temp_state state);
 
 
+int sysmobts_mgr_calib_init(struct sysmobts_mgr_instance *mgr);
+int sysmobts_mgr_calib_run(struct sysmobts_mgr_instance *mgr);
+
+
 extern void *tall_mgr_ctx;
 
 #endif
diff --git a/src/osmo-bts-sysmo/misc/sysmobts_mgr_calib.c b/src/osmo-bts-sysmo/misc/sysmobts_mgr_calib.c
index 4313e68c..165b7311 100644
--- a/src/osmo-bts-sysmo/misc/sysmobts_mgr_calib.c
+++ b/src/osmo-bts-sysmo/misc/sysmobts_mgr_calib.c
@@ -27,6 +27,8 @@
 #include <osmocom/core/logging.h>
 #include <osmocom/core/select.h>
 
+#include <osmocom/ctrl/control_cmd.h>
+
 #include <osmocom/gsm/ipa.h>
 #include <osmocom/gsm/protocol/ipaccess.h>
 
@@ -36,6 +38,221 @@
 
 static void bts_updown_cb(struct ipa_client_conn *link, int up);
 
+enum calib_state {
+	CALIB_INITIAL,
+	CALIB_CTR_RESET,
+	CALIB_CTR_WAIT,
+	CALIB_COR_SET,
+};
+
+static void send_ctrl_cmd(struct sysmobts_mgr_instance *mgr,
+			struct msgb *msg)
+{
+	ipa_prepend_header_ext(msg, IPAC_PROTO_EXT_CTRL);
+	ipa_prepend_header(msg, IPAC_PROTO_OSMO);
+	ipa_client_conn_send(mgr->calib.bts_conn, msg);
+}
+
+static void send_set_ctrl_cmd(struct sysmobts_mgr_instance *mgr,
+				const char *key, const char *val)
+{
+	struct msgb *msg;
+	int ret;
+
+	msg = msgb_alloc_headroom(1024, 128, "CTRL SET");
+	ret = snprintf((char *) msg->data, 4096, "SET %u %s %s",
+			mgr->calib.last_seqno++, key, val);
+	msg->l2h = msgb_put(msg, ret);
+	return send_ctrl_cmd(mgr, msg);
+}
+
+static void send_get_ctrl_cmd(struct sysmobts_mgr_instance *mgr,
+				const char *key)
+{
+	struct msgb *msg;
+	int ret;
+
+	msg = msgb_alloc_headroom(1024, 128, "CTRL GET");
+	ret = snprintf((char *) msg->data, 4096, "GET %u %s",
+			mgr->calib.last_seqno++, key);
+	msg->l2h = msgb_put(msg, ret);
+	return send_ctrl_cmd(mgr, msg);
+}
+
+int sysmobts_mgr_calib_run(struct sysmobts_mgr_instance *mgr)
+{
+	if (!mgr->calib.is_up) {
+		LOGP(DCALIB, LOGL_ERROR, "Control interface not connected.\n");
+		return -1;
+	}
+
+	if (mgr->calib.state != CALIB_INITIAL) {
+		LOGP(DCALIB, LOGL_ERROR, "Calib is already in progress.\n");
+		return -2;
+	}
+
+	send_set_ctrl_cmd(mgr, "trx.0.clock-info", "1");
+	mgr->calib.state = CALIB_CTR_RESET;
+	return 0;
+}
+
+static void calib_state_reset(struct sysmobts_mgr_instance *mgr)
+{
+	mgr->calib.state = CALIB_INITIAL;
+	osmo_timer_del(&mgr->calib.timer);
+}
+
+static void calib_get_clock_err_cb(void *_data)
+{
+	struct sysmobts_mgr_instance *mgr = _data;
+
+	send_get_ctrl_cmd(mgr, "trx.0.clock-info");
+}
+
+static void handle_ctrl_reset_resp(
+				struct sysmobts_mgr_instance *mgr,
+				struct ctrl_cmd *cmd)
+{
+	if (strcmp(cmd->variable, "trx.0.clock-info") != 0) {
+		LOGP(DCALIB, LOGL_ERROR,
+			"Unexpected variable: %s\n", cmd->variable);
+		calib_state_reset(mgr);
+		return;
+	}
+
+	if (strcmp(cmd->reply, "success") != 0) {
+		LOGP(DCALIB, LOGL_ERROR,
+			"Unexpected reply: %s\n", cmd->variable);
+		calib_state_reset(mgr);
+		return;
+	}
+
+	mgr->calib.state = CALIB_CTR_WAIT;
+	mgr->calib.timer.cb = calib_get_clock_err_cb;
+	mgr->calib.timer.data = mgr;
+	osmo_timer_schedule(&mgr->calib.timer, 60, 0);
+}
+
+static void handle_ctrl_get_resp(
+				struct sysmobts_mgr_instance *mgr,
+				struct ctrl_cmd *cmd)
+{
+	char *saveptr = NULL;
+	char *clk_cur;
+	char *clk_src;
+	char *cal_err;
+	char *cal_res;
+	char *cal_src;
+
+	if (strcmp(cmd->variable, "trx.0.clock-info") != 0) {
+		LOGP(DCALIB, LOGL_ERROR,
+			"Unexpected variable: %s\n", cmd->variable);
+		calib_state_reset(mgr);
+		return;
+	}
+
+	clk_cur = strtok_r(cmd->reply, ",", &saveptr);
+	clk_src = strtok_r(NULL, ",", &saveptr);
+	cal_err = strtok_r(NULL, ",", &saveptr);
+	cal_res = strtok_r(NULL, ",", &saveptr);
+	cal_src = strtok_r(NULL, ",", &saveptr);
+
+	if (!clk_cur || !clk_src || !cal_err || !cal_res || !cal_src) {
+		LOGP(DCALIB, LOGL_ERROR, "Parse error on clock-info reply\n");
+		calib_state_reset(mgr);
+		return;
+
+	}
+	LOGP(DCALIB, LOGL_NOTICE,
+		"Calibration CUR(%s) SRC(%s) ERR(%s) RES(%s) SRC(%s)\n",
+		clk_cur, clk_src, cal_err, cal_res, cal_src);
+
+	if (strcmp(cal_res, "0") == 0) {
+		LOGP(DCALIB, LOGL_ERROR, "Invalid clock resolution. Giving up\n");
+		calib_state_reset(mgr);
+		return;
+	}
+
+	/* Now we can finally set the new value */
+	send_set_ctrl_cmd(mgr, "trx.0.clock-correction", cal_err);
+	mgr->calib.state = CALIB_COR_SET;
+}
+
+static void handle_ctrl_set_cor(
+			struct sysmobts_mgr_instance *mgr,
+			struct ctrl_cmd *cmd)
+{
+	if (strcmp(cmd->variable, "trx.0.clock-correction") != 0) {
+		LOGP(DCALIB, LOGL_ERROR,
+			"Unexpected variable: %s\n", cmd->variable);
+		calib_state_reset(mgr);
+		return;
+	}
+
+	if (strcmp(cmd->reply, "success") != 0) {
+		LOGP(DCALIB, LOGL_ERROR,
+			"Unexpected reply: %s\n", cmd->variable);
+		calib_state_reset(mgr);
+		return;
+	}
+
+	LOGP(DCALIB, LOGL_NOTICE,
+		"Calibration process completed\n");
+	mgr->calib.state = CALIB_INITIAL;
+}
+
+static void handle_ctrl(struct sysmobts_mgr_instance *mgr, struct msgb *msg)
+{
+	struct ctrl_cmd *cmd = ctrl_cmd_parse(tall_mgr_ctx, msg);
+	if (!cmd) {
+		LOGP(DCALIB, LOGL_ERROR, "Failed to parse command/response\n");
+		return;
+	}
+
+	switch (cmd->type) {
+	case CTRL_TYPE_GET_REPLY:
+		switch (mgr->calib.state) {
+		case CALIB_CTR_WAIT:
+			handle_ctrl_get_resp(mgr, cmd);
+			break;
+		default:
+			LOGP(DCALIB, LOGL_ERROR,
+				"Unhandled response in state: %d %s/%s\n",
+				mgr->calib.state, cmd->variable, cmd->reply);
+			calib_state_reset(mgr);
+			break;
+		};
+		break;
+	case CTRL_TYPE_SET_REPLY:
+		switch (mgr->calib.state) {
+		case CALIB_CTR_RESET:
+			handle_ctrl_reset_resp(mgr, cmd);
+			break;
+		case CALIB_COR_SET:
+			handle_ctrl_set_cor(mgr, cmd);
+			break;
+		default:
+			LOGP(DCALIB, LOGL_ERROR,
+				"Unhandled response in state: %d %s/%s\n",
+				mgr->calib.state, cmd->variable, cmd->reply);
+			calib_state_reset(mgr);
+			break;
+		};
+		break;
+	case CTRL_TYPE_TRAP:
+		/* ignore any form of trap */
+		break;
+	default:
+		LOGP(DCALIB, LOGL_ERROR,
+			"Unhandled CTRL response: %d. Resetting state\n",
+			cmd->type);
+		calib_state_reset(mgr);
+		break;
+	}
+
+	talloc_free(cmd);
+}
+
 /* Schedule a connect towards the BTS */
 static void schedule_bts_connect(struct sysmobts_mgr_instance *mgr)
 {
@@ -62,14 +279,15 @@ static int bts_read_cb(struct ipa_client_conn *link, struct msgb *msg)
 {
 	int rc;
 	struct ipaccess_head *hh = (struct ipaccess_head *) msgb_l1(msg);
+	struct ipaccess_head_ext *hh_ext;
 
-	DEBUGP(DLCTRL, "Received data from BTS: %s\n",
+	DEBUGP(DCALIB, "Received data from BTS: %s\n",
 		osmo_hexdump(msgb_data(msg), msgb_length(msg)));
 
 	/* regular message handling */
 	rc = msg_verify_ipa_structure(msg);
 	if (rc < 0) {
-		LOGP(DLCTRL, LOGL_ERROR,
+		LOGP(DCALIB, LOGL_ERROR,
 			"Invalid IPA message from BTS (rc=%d)\n", rc);
 		goto err;
 	}
@@ -80,8 +298,20 @@ static int bts_read_cb(struct ipa_client_conn *link, struct msgb *msg)
 		ipa_ccm_rcvmsg_bts_base(msg, link->ofd);
 		msgb_free(msg);
 		break;
+	case IPAC_PROTO_OSMO:
+		hh_ext = (struct ipaccess_head_ext *) hh->data;
+		switch (hh_ext->proto) {
+		case IPAC_PROTO_EXT_CTRL:
+			handle_ctrl(link->data, msg);
+			break;
+		default:
+			LOGP(DCALIB, LOGL_NOTICE,
+				"Unhandled osmo ID %u from BTS\n", hh_ext->proto);
+		};
+		msgb_free(msg);
+		break;
 	default:
-		LOGP(DLCTRL, LOGL_NOTICE,
+		LOGP(DCALIB, LOGL_NOTICE,
 		     "Unhandled stream ID %u from BTS\n", hh->proto);
 		msgb_free(msg);
 		break;
@@ -102,9 +332,12 @@ static void bts_updown_cb(struct ipa_client_conn *link, int up)
 
 	if (up) {
 		mgr->calib.is_up = 1;
+		mgr->calib.last_seqno = 0;
+		calib_state_reset(mgr);
 	} else {
 		mgr->calib.is_up = 0;
 		schedule_bts_connect(mgr);
+		calib_state_reset(mgr);
 	}
 }
 
diff --git a/src/osmo-bts-sysmo/misc/sysmobts_mgr_vty.c b/src/osmo-bts-sysmo/misc/sysmobts_mgr_vty.c
index 21443774..00176104 100644
--- a/src/osmo-bts-sysmo/misc/sysmobts_mgr_vty.c
+++ b/src/osmo-bts-sysmo/misc/sysmobts_mgr_vty.c
@@ -424,6 +424,18 @@ DEFUN(show_mgr, show_mgr_cmd, "show manager",
 	return CMD_SUCCESS;
 }
 
+DEFUN(calibrate_trx, calibrate_trx_cmd,
+      "trx 0 calibrate-clock",
+      "Transceiver commands\n" "Transceiver 0\n"
+      "Calibrate clock against GPS PPS\n")
+{
+	if (sysmobts_mgr_calib_run(s_mgr) < 0) {
+		vty_out(vty, "%%Failed to start calibration.%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
 static void register_limit(int limit)
 {
 	install_element(limit, &cfg_thresh_warning_cmd);
@@ -464,6 +476,8 @@ int sysmobts_mgr_vty_init(void)
 
 	install_element_ve(&show_mgr_cmd);
 
+	install_element(ENABLE_NODE, &calibrate_trx_cmd);
+
 	install_node(&mgr_node, config_write_mgr);
 	install_element(CONFIG_NODE, &cfg_mgr_cmd);
 	vty_install_default(MGR_NODE);