From d397129ebafc4b23f26f5537b0e5dce2a9b481eb Mon Sep 17 00:00:00 2001
From: Andreas Eversberg <jolly@eversberg.eu>
Date: Sun, 14 Jan 2024 15:04:00 +0100
Subject: [PATCH] Add command and client function to change transmitted Sa bits

Change-Id: I2608af7bbb8092fddd68d4f3bb36b10a1100ce0f
---
 include/osmocom/e1d/proto_clnt.h |  1 +
 src/ctl.c                        | 37 ++++++++++++++++++++++++++++++++
 src/e1d.h                        |  2 ++
 src/intf_line.c                  |  1 +
 src/mux_demux.c                  | 11 ++++++++--
 src/proto_clnt.c                 | 28 ++++++++++++++++++++++++
 6 files changed, 78 insertions(+), 2 deletions(-)

diff --git a/include/osmocom/e1d/proto_clnt.h b/include/osmocom/e1d/proto_clnt.h
index 26bd44e..b5be2ce 100644
--- a/include/osmocom/e1d/proto_clnt.h
+++ b/include/osmocom/e1d/proto_clnt.h
@@ -43,6 +43,7 @@ int osmo_e1dp_client_ts_query(struct osmo_e1dp_client *clnt,
 	uint8_t intf, uint8_t line, uint8_t ts);
 int osmo_e1dp_client_line_config(struct osmo_e1dp_client *clnt,
 	uint8_t intf, uint8_t line, enum osmo_e1dp_line_mode mode);
+int osmo_e1dp_client_set_sa_bits(struct osmo_e1dp_client *clnt, uint8_t intf, uint8_t line, uint8_t sa_bits);
 int osmo_e1dp_client_ts_open(struct osmo_e1dp_client *clnt,
 	uint8_t intf, uint8_t line, uint8_t ts,
 	enum osmo_e1dp_ts_mode mode, uint16_t read_bufsize);
diff --git a/src/ctl.c b/src/ctl.c
index 8ed4a69..331f2ad 100644
--- a/src/ctl.c
+++ b/src/ctl.c
@@ -430,6 +430,37 @@ _e1d_ctl_ts_open(void *data, struct msgb *msgb, struct msgb *rmsgb, int *rfd)
 	return 0;
 }
 
+static int
+_e1d_ctl_sabits(void *data, struct msgb *msgb, struct msgb *rmsgb, int *rfd)
+{
+	struct e1_daemon *e1d = (struct e1_daemon *)data;
+	struct osmo_e1dp_msg_hdr *hdr = msgb_l1(msgb);
+	uint8_t sa_bits = *(uint8_t *)msgb_l2(msgb);
+	struct e1_intf *intf = NULL;
+	struct e1_line *line = NULL;
+
+	/* Process query and find timeslot */
+	intf = e1d_find_intf(e1d, hdr->intf);
+	if (!intf) {
+		LOGP(DE1D, LOGL_NOTICE, "Client request for non-existant Interface %u\n", hdr->intf);
+		return 0;
+	}
+
+	line = e1_intf_find_line(intf, hdr->line);
+	if (!line) {
+		LOGPIF(intf, DE1D, LOGL_NOTICE, "Client request for non-existant line %u\n", hdr->line);
+		return 0;
+	}
+
+	line->ts0.tx_frame = ((sa_bits & 0x80) >> 7) | /* Bit 7 -> Sa8 */
+			     ((sa_bits & 0x40) >> 5) | /* Bit 6 -> Sa7 */
+			     ((sa_bits & 0x01) << 2) | /* Bit 0 -> Sa6 */
+			     ((sa_bits & 0x20) >> 2) | /* Bit 5 -> Sa5 */
+			     (sa_bits & 0x10); /* Bit 4 -> Sa4 */
+
+	return 0;
+}
+
 
 struct osmo_e1dp_server_handler e1d_ctl_handlers[] = {
 	{
@@ -462,5 +493,11 @@ struct osmo_e1dp_server_handler e1d_ctl_handlers[] = {
 		.payload_len = sizeof(struct osmo_e1dp_ts_config),
 		.fn = _e1d_ctl_ts_open,
 	},
+	{
+		.type = E1DP_CMD_SABITS,
+		.flags = E1DP_SF_INTF_REQ | E1DP_SF_LINE_REQ,
+		.payload_len = sizeof(uint8_t),
+		.fn = _e1d_ctl_sabits,
+	},
 	{ /* guard */ },
 };
diff --git a/src/e1d.h b/src/e1d.h
index 4b70c0f..bf1cbdf 100644
--- a/src/e1d.h
+++ b/src/e1d.h
@@ -148,6 +148,8 @@ struct e1_line {
 		uint8_t prev_errmask;
 		/*! timer to re-set the rx_crc4_err and rx_alarm above */
 		struct osmo_timer_list timer;
+		/*! current transmitting frame with Sa bits */
+		uint8_t tx_frame;
 		/*! last received frame with Sa bits */
 		uint8_t rx_frame;
 	} ts0;
diff --git a/src/intf_line.c b/src/intf_line.c
index b3dafff..8eac84d 100644
--- a/src/intf_line.c
+++ b/src/intf_line.c
@@ -272,6 +272,7 @@ e1_line_new(struct e1_intf *intf, int line_id, void *drv_data)
 	line->intf = intf;
 	line->drv_data = drv_data;
 	line->mode = E1_LINE_MODE_CHANNELIZED;
+	line->ts0.tx_frame = 0xff;
 	line->ts0.rx_frame = 0xff;
 
 	for (int i = 0; i < 32; i++)
diff --git a/src/mux_demux.c b/src/mux_demux.c
index 71374ef..dd8ca45 100644
--- a/src/mux_demux.c
+++ b/src/mux_demux.c
@@ -149,11 +149,18 @@ _e1_ts_read(struct e1_ts *ts, uint8_t *buf, size_t len)
 static void
 _e1_line_mux_out_channelized(struct e1_line *line, uint8_t *buf, int fts)
 {
+	struct e1_ts *ts;
+
 	OSMO_ASSERT(line->mode == E1_LINE_MODE_CHANNELIZED);
 
-	/* Scan timeslots */
+	/* Fill timeslot 0 */
+	ts = &line->ts[0];
+	for (int i = 0; i < fts; i++)
+		buf[(i*32)] = line->ts0.tx_frame;
+
+	/* Scan timeslots 1..31 */
 	for (int tsn = 1; tsn < 32; tsn++) {
-		struct e1_ts *ts = &line->ts[tsn];
+		ts = &line->ts[tsn];
 		uint8_t buf_ts[fts];
 		int l;
 
diff --git a/src/proto_clnt.c b/src/proto_clnt.c
index 99d1cc2..854b958 100644
--- a/src/proto_clnt.c
+++ b/src/proto_clnt.c
@@ -394,6 +394,34 @@ osmo_e1dp_client_line_config(struct osmo_e1dp_client *clnt,
 	return 0;
 }
 
+/*! Set Sa-bits of a specific E1 line in osmo-e1d.
+ *  \param[in] clnt Client previously returned from osmo_e1dp_client_create().
+ *  \param[in] intf E1 interface number to configure.
+ *  \param[in] line E1 line number (within interface) to configure.
+ *  \param[in] sa_bits Sa bits to set on line.
+ *  \returns zero in case of success; negative in case of error. */
+int
+osmo_e1dp_client_set_sa_bits(struct osmo_e1dp_client *clnt, uint8_t intf, uint8_t line, uint8_t sa_bits)
+{
+	struct msgb *msgb;
+	struct osmo_e1dp_msg_hdr hdr;
+	int rc;
+
+	memset(&hdr, 0x00, sizeof(struct osmo_e1dp_msg_hdr));
+	hdr.type = E1DP_CMD_SABITS;
+	hdr.intf = intf;
+	hdr.line = line;
+	hdr.ts = E1DP_INVALID;
+
+	rc = _e1dp_client_query_base(clnt, &hdr, &sa_bits, 1, &msgb, NULL);
+	if (rc)
+		return rc;
+
+	msgb_free(msgb);
+
+	return 0;
+}
+
 static int
 _client_ts_open(struct osmo_e1dp_client *clnt,
 	uint8_t intf, uint8_t line, uint8_t ts,