host/trxcon/scheduler: add basic clock counter

The core of scheduler is a simple clock counter, which relays
on system time for now. One was a bit simplified and migrated
from OsmoBTS.

Due to system time is not an ideal clock source, the counter
should be periodically corrected by clock indications from BTS.

Change-Id: I27d85bd3e2c8bca3f876f73517027b9fe43c9825

9 files changed, 269 insertions, 3 deletions

diff --git a/src/host/layer23/.gitignore b/src/host/layer23/.gitignore
index 8fb93f73..59601be8 100644
--- a/src/host/layer23/.gitignore
+++ b/src/host/layer23/.gitignore
@@ -19,7 +19,6 @@ config.status
 
 # build by-products
 *.o
-*.a
 
 # various
 *.sw?
diff --git a/src/host/trxcon/.gitignore b/src/host/trxcon/.gitignore
index d6b28ee9..fe90e43c 100644
--- a/src/host/trxcon/.gitignore
+++ b/src/host/trxcon/.gitignore
@@ -18,6 +18,7 @@ version.h
 
 # build by-products
 *.o
+*.a
 
 trxcon
 
diff --git a/src/host/trxcon/Makefile.am b/src/host/trxcon/Makefile.am
index 869ed8ba..de120297 100644
--- a/src/host/trxcon/Makefile.am
+++ b/src/host/trxcon/Makefile.am
@@ -28,6 +28,11 @@ trxcon_SOURCES = \
 	trxcon.c \
 	$(NULL)
 
+# Scheduler
+trxcon_SOURCES += \
+	sched_clck.c \
+	$(NULL)
+
 trxcon_LDADD = \
 	$(LIBOSMOCORE_LIBS) \
 	$(LIBOSMOGSM_LIBS) \
diff --git a/src/host/trxcon/logging.c b/src/host/trxcon/logging.c
index 28e6776b..3381c6a4 100644
--- a/src/host/trxcon/logging.c
+++ b/src/host/trxcon/logging.c
@@ -46,6 +46,12 @@ static struct log_info_cat trx_log_info_cat[] = {
 		.color = "\033[1;33m",
 		.enabled = 1, .loglevel = LOGL_NOTICE,
 	},
+	[DSCH] = {
+		.name = "DSCH",
+		.description = "Scheduler",
+		.color = "\033[1;36m",
+		.enabled = 1, .loglevel = LOGL_NOTICE,
+	},
 };
 
 static const struct log_info trx_log_info = {
diff --git a/src/host/trxcon/logging.h b/src/host/trxcon/logging.h
index 4d7cea06..52afd4ba 100644
--- a/src/host/trxcon/logging.h
+++ b/src/host/trxcon/logging.h
@@ -2,12 +2,13 @@
 
 #include <osmocom/core/logging.h>
 
-#define DEBUG_DEFAULT "DAPP:DL1C:DTRX"
+#define DEBUG_DEFAULT "DAPP:DL1C:DTRX:DSCH"
 
 enum {
 	DAPP,
 	DL1C,
 	DTRX,
+	DSCH,
 };
 
 int trx_log_init(const char *category_mask);
diff --git a/src/host/trxcon/sched_clck.c b/src/host/trxcon/sched_clck.c
new file mode 100644
index 00000000..31f3ef2c
--- /dev/null
+++ b/src/host/trxcon/sched_clck.c
@@ -0,0 +1,209 @@
+/*
+ * OsmocomBB <-> SDR connection bridge
+ * TDMA scheduler: clock synchronization
+ *
+ * (C) 2013 by Andreas Eversberg <jolly@eversberg.eu>
+ * (C) 2015 by Alexander Chemeris <Alexander.Chemeris@fairwaves.co>
+ * (C) 2015 by Harald Welte <laforge@gnumonks.org>
+ *
+ * All Rights Reserved
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU Affero General Public License as published by
+ * the Free Software Foundation; either version 3 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 Affero General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/bits.h>
+#include <osmocom/core/fsm.h>
+#include <osmocom/gsm/a5.h>
+
+#include "scheduler.h"
+#include "logging.h"
+#include "trx_if.h"
+#include "trxcon.h"
+
+#define FRAME_DURATION_uS	4615
+#define MAX_FN_SKEW		50
+#define TRX_LOSS_FRAMES	400
+
+extern struct osmo_fsm_inst *trxcon_fsm;
+
+static void sched_clck_tick(void *data)
+{
+	struct trx_sched *sched = (struct trx_sched *) data;
+	struct trx_instance *trx = (struct trx_instance *) sched->data;
+
+	struct timeval tv_now, *tv_clock;
+	int32_t elapsed;
+
+	/* Check if transceiver is still alive */
+	if (sched->fn_counter_lost++ == TRX_LOSS_FRAMES) {
+		LOGP(DSCH, LOGL_NOTICE, "No more clock from transceiver\n");
+
+		osmo_fsm_inst_dispatch(trxcon_fsm, SCH_EVENT_CLCK_LOSS, trx);
+		sched->state = SCH_CLCK_STATE_WAIT;
+
+		return;
+	}
+
+	/* Get actual / previous frame time */
+	gettimeofday(&tv_now, NULL);
+	tv_clock = &sched->clock;
+
+	elapsed = (tv_now.tv_sec - tv_clock->tv_sec) * 1000000
+		+ (tv_now.tv_usec - tv_clock->tv_usec);
+
+	/* If someone played with clock, or if the process stalled */
+	if (elapsed > FRAME_DURATION_uS * MAX_FN_SKEW || elapsed < 0) {
+		LOGP(DSCH, LOGL_NOTICE, "PC clock skew: "
+			"elapsed uS %d\n", elapsed);
+
+		osmo_fsm_inst_dispatch(trxcon_fsm, SCH_EVENT_CLCK_LOSS, trx);
+		sched->state = SCH_CLCK_STATE_WAIT;
+
+		return;
+	}
+
+	/* Schedule next FN clock */
+	while (elapsed > FRAME_DURATION_uS / 2) {
+		tv_clock->tv_usec += FRAME_DURATION_uS;
+		elapsed -= FRAME_DURATION_uS;
+
+		if (tv_clock->tv_usec >= 1000000) {
+			tv_clock->tv_sec++;
+			tv_clock->tv_usec -= 1000000;
+		}
+
+		sched->fn_counter_proc = (sched->fn_counter_proc + 1)
+			% GSM_HYPERFRAME;
+
+		/* Call frame callback */
+		if (sched->clock_cb)
+			sched->clock_cb(sched);
+	}
+
+	osmo_timer_schedule(&sched->clock_timer, 0,
+		FRAME_DURATION_uS - elapsed);
+}
+
+static void sched_clck_correct(struct trx_sched *sched,
+	struct timeval *tv_now, uint32_t fn)
+{
+	sched->fn_counter_proc = fn;
+
+	/* Call frame callback */
+	if (sched->clock_cb)
+		sched->clock_cb(sched);
+
+	/* Schedule first FN clock */
+	memcpy(&sched->clock, tv_now, sizeof(struct timeval));
+	memset(&sched->clock_timer, 0, sizeof(sched->clock_timer));
+
+	sched->clock_timer.cb = sched_clck_tick;
+	sched->clock_timer.data = sched;
+	osmo_timer_schedule(&sched->clock_timer, 0, FRAME_DURATION_uS);
+}
+
+int sched_clck_handle(struct trx_sched *sched, uint32_t fn)
+{
+	struct trx_instance *trx = (struct trx_instance *) sched->data;
+	struct timeval tv_now, *tv_clock;
+	int32_t elapsed, elapsed_fn;
+
+	/* Reset lost counter */
+	sched->fn_counter_lost = 0;
+
+	/* Get actual / previous frame time */
+	gettimeofday(&tv_now, NULL);
+	tv_clock = &sched->clock;
+
+	/* If this is the first CLCK IND */
+	if (sched->state == SCH_CLCK_STATE_WAIT) {
+		sched_clck_correct(sched, &tv_now, fn);
+
+		LOGP(DSCH, LOGL_NOTICE, "Initial clock received: fn=%u\n", fn);
+		osmo_fsm_inst_dispatch(trxcon_fsm, SCH_EVENT_CLCK_IND, trx);
+		sched->state = SCH_CLCK_STATE_OK;
+
+		return 0;
+	}
+
+	osmo_timer_del(&sched->clock_timer);
+
+	/* Calculate elapsed time / frames since last processed fn */
+	elapsed = (tv_now.tv_sec - tv_clock->tv_sec) * 1000000
+		+ (tv_now.tv_usec - tv_clock->tv_usec);
+	elapsed_fn = (fn + GSM_HYPERFRAME - sched->fn_counter_proc)
+		% GSM_HYPERFRAME;
+
+	if (elapsed_fn >= 135774)
+		elapsed_fn -= GSM_HYPERFRAME;
+
+	/* Check for max clock skew */
+	if (elapsed_fn > MAX_FN_SKEW || elapsed_fn < -MAX_FN_SKEW) {
+		LOGP(DSCH, LOGL_NOTICE, "GSM clock skew: old fn=%u, "
+			"new fn=%u\n", sched->fn_counter_proc, fn);
+
+		sched_clck_correct(sched, &tv_now, fn);
+		return 0;
+	}
+
+	LOGP(DSCH, LOGL_INFO, "GSM clock jitter: %d\n",
+		elapsed_fn * FRAME_DURATION_uS - elapsed);
+
+	/* Too many frames have been processed already */
+	if (elapsed_fn < 0) {
+		/**
+		 * Set clock to the time or last FN should
+		 * have been transmitted
+		 */
+		tv_clock->tv_sec = tv_now.tv_sec;
+		tv_clock->tv_usec = tv_now.tv_usec +
+			(0 - elapsed_fn) * FRAME_DURATION_uS;
+
+		if (tv_clock->tv_usec >= 1000000) {
+			tv_clock->tv_sec++;
+			tv_clock->tv_usec -= 1000000;
+		}
+
+		/* Set time to the time our next FN has to be transmitted */
+		osmo_timer_schedule(&sched->clock_timer, 0,
+			FRAME_DURATION_uS * (1 - elapsed_fn));
+
+		return 0;
+	}
+
+	/* Transmit what we still need to transmit */
+	while (fn != sched->fn_counter_proc) {
+		sched->fn_counter_proc = (sched->fn_counter_proc + 1)
+			% GSM_HYPERFRAME;
+
+		/* Call frame callback */
+		if (sched->clock_cb)
+			sched->clock_cb(sched);
+	}
+
+	/* Schedule next FN to be transmitted */
+	memcpy(tv_clock, &tv_now, sizeof(struct timeval));
+	osmo_timer_schedule(&sched->clock_timer, 0, FRAME_DURATION_uS);
+
+	return 0;
+}
diff --git a/src/host/trxcon/scheduler.h b/src/host/trxcon/scheduler.h
new file mode 100644
index 00000000..0783e40a
--- /dev/null
+++ b/src/host/trxcon/scheduler.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <stdint.h>
+#include <time.h>
+
+#include <osmocom/core/timer.h>
+
+#define GSM_SUPERFRAME		(26 * 51)
+#define GSM_HYPERFRAME		(2048 * GSM_SUPERFRAME)
+
+enum tdma_sched_clck_state {
+	SCH_CLCK_STATE_WAIT,
+	SCH_CLCK_STATE_OK,
+};
+
+/* Forward structure declaration */
+struct trx_sched;
+
+/*! \brief One scheduler instance */
+struct trx_sched {
+	/*! \brief Clock state */
+	uint8_t state;
+	/*! \brief Local clock source */
+	struct timeval clock;
+	/*! \brief Count of processed frames */
+	uint32_t fn_counter_proc;
+	/*! \brief Frame counter */
+	uint32_t fn_counter_lost;
+	/*! \brief Frame callback timer */
+	struct osmo_timer_list clock_timer;
+	/*! \brief Frame callback */
+	void (*clock_cb)(struct trx_sched *sched);
+	/*! \brief Private data (e.g. pointer to trx instance) */
+	void *data;
+};
+
+int sched_clck_handle(struct trx_sched *sched, uint32_t fn);
diff --git a/src/host/trxcon/trxcon.c b/src/host/trxcon/trxcon.c
index a90d038d..ace2f79a 100644
--- a/src/host/trxcon/trxcon.c
+++ b/src/host/trxcon/trxcon.c
@@ -104,6 +104,8 @@ static void trxcon_fsm_managed_action(struct osmo_fsm_inst *fi,
 		break;
 	case TRX_EVENT_RSP_ERROR:
 	case TRX_EVENT_OFFLINE:
+	case SCH_EVENT_CLCK_IND:
+	case SCH_EVENT_CLCK_LOSS:
 		/* TODO: notify L2 & L3 about that */
 		break;
 	default:
@@ -125,7 +127,9 @@ static struct osmo_fsm_state trxcon_fsm_states[] = {
 			GEN_MASK(L1CTL_EVENT_RESET_REQ) |
 			GEN_MASK(TRX_EVENT_RESET_IND) |
 			GEN_MASK(TRX_EVENT_RSP_ERROR) |
-			GEN_MASK(TRX_EVENT_OFFLINE)),
+			GEN_MASK(TRX_EVENT_OFFLINE) |
+			GEN_MASK(SCH_EVENT_CLCK_IND) |
+			GEN_MASK(SCH_EVENT_CLCK_LOSS)),
 		.out_state_mask = GEN_MASK(TRXCON_STATE_IDLE),
 		.name = "MANAGED",
 		.action = trxcon_fsm_managed_action,
diff --git a/src/host/trxcon/trxcon.h b/src/host/trxcon/trxcon.h
index 9535578e..c266eaee 100644
--- a/src/host/trxcon/trxcon.h
+++ b/src/host/trxcon/trxcon.h
@@ -18,4 +18,8 @@ enum trxcon_fsm_events {
 	TRX_EVENT_RESET_IND,
 	TRX_EVENT_RSP_ERROR,
 	TRX_EVENT_OFFLINE,
+
+	/* Scheduler specific events */
+	SCH_EVENT_CLCK_IND,
+	SCH_EVENT_CLCK_LOSS,
 };