[nrf fromtree] modem: cmux: Implement Modem Status Command

Implement Modem Status Command(MSC) and a per DLC flow
control by using it.

Send flow control signals when our input buffer don't fit
full frame anymore.
Stop TX if we have received from controls on MSC.

Signed-off-by: Seppo Takalo <[email protected]>
(cherry picked from commit 1f996d0)

Author: SeppoTakalo

include/zephyr/modem/cmux.h

@@ -120,6 +120,10 @@ struct modem_cmux_dlci {
 #if CONFIG_MODEM_STATS
 	struct modem_stats_buffer receive_buf_stats;
 #endif
+	/* Flow control */
+	bool flow_control : 1;
+	bool rx_full : 1;
+	bool msc_sent : 1;
 };
 
 struct modem_cmux_frame {

subsys/modem/modem_cmux.c

@@ -81,6 +81,29 @@ struct modem_cmux_command {
 	uint8_t value[];
 };
 
+struct modem_cmux_msc_signals {
+	uint8_t ea: 1;    /**< Last octet, always 1 */
+	uint8_t fc: 1;    /**< Flow Control */
+	uint8_t rtc: 1;   /**< Ready to Communicate */
+	uint8_t rtr: 1;   /**< Ready to Transmit */
+	uint8_t res_0: 2; /**< Reserved, set to zero */
+	uint8_t ic: 1;    /**< Incoming call indicator */
+	uint8_t dv: 1;    /**< Data Valid */
+};
+struct modem_cmux_msc_addr {
+	uint8_t ea: 1;                         /**< Last octet, always 1 */
+	uint8_t pad_one: 1;                    /**< Set to 1 */
+	uint8_t dlci_address: 6;               /**< DLCI channel address */
+};
+
+struct modem_cmux_command_msc {
+	struct modem_cmux_command command;
+	uint8_t value[2];
+};
+
+static struct modem_cmux_dlci *modem_cmux_find_dlci(struct modem_cmux *cmux, uint8_t dlci_address);
+static void modem_cmux_dlci_notify_transmit_idle(struct modem_cmux *cmux);
+
 static int modem_cmux_wrap_command(struct modem_cmux_command **command, const uint8_t *data,
 				   uint16_t data_len)
 {
@@ -122,6 +145,53 @@ static struct modem_cmux_command *modem_cmux_command_wrap(const uint8_t *data)
 	return (struct modem_cmux_command *)data;
 }
 
+static struct modem_cmux_msc_signals modem_cmux_msc_signals_decode(const uint8_t byte)
+{
+	struct modem_cmux_msc_signals signals;
+
+	/* 3GPP TS 27.010 MSC signals octet:
+	 * |0  |1 |2  |3  |4 |5 |6 |7 |
+	 * |EA |FC|RTC|RTR|0 |0 |IC|DV|
+	 */
+	signals.ea = (byte & BIT(0)) ? 1 : 0;
+	signals.fc = (byte & BIT(1)) ? 1 : 0;
+	signals.rtc = (byte & BIT(2)) ? 1 : 0;
+	signals.rtr = (byte & BIT(3)) ? 1 : 0;
+	signals.ic = (byte & BIT(6)) ? 1 : 0;
+	signals.dv = (byte & BIT(7)) ? 1 : 0;
+
+	return signals;
+}
+
+static uint8_t modem_cmux_msc_signals_encode(const struct modem_cmux_msc_signals signals)
+{
+	return (signals.ea ? BIT(0) : 0) | (signals.fc ? BIT(1) : 0) |
+	       (signals.rtc ? BIT(2) : 0) | (signals.rtr ? BIT(3) : 0) |
+	       (signals.ic ? BIT(6) : 0) | (signals.dv ? BIT(7) : 0);
+}
+
+static struct modem_cmux_msc_addr modem_cmux_msc_addr_decode(const uint8_t byte)
+{
+	struct modem_cmux_msc_addr addr;
+
+	/* 3GPP TS 27.010 MSC address octet:
+	 * |0  |1 |2 |3 |4 |5 |6 |7 |
+	 * |EA |1 |      DLCI       |
+	 */
+	addr.ea = (byte & BIT(0)) ? 1 : 0;
+	addr.pad_one = 1;
+	addr.dlci_address = (byte >> 2) & 0x3F;
+
+	return addr;
+}
+
+static uint8_t modem_cmux_msc_addr_encode(const struct modem_cmux_msc_addr a)
+{
+	return (a.ea ? BIT(0) : 0) | BIT(1) |
+	       ((a.dlci_address & 0x3F) << 2);
+}
+
+
 #if CONFIG_MODEM_CMUX_LOG_FRAMES
 static const char *modem_cmux_frame_type_to_str(enum modem_cmux_frame_types frame_type)
 {
@@ -426,10 +496,92 @@ static void modem_cmux_acknowledge_received_frame(struct modem_cmux *cmux)
 	}
 }
 
+static void modem_cmux_send_msc(struct modem_cmux *cmux, struct modem_cmux_dlci *dlci)
+{
+	if (cmux == NULL || dlci == NULL) {
+		return;
+	}
+
+	struct modem_cmux_msc_addr addr = {
+		.ea = 1,
+		.pad_one = 1,
+		.dlci_address = dlci->dlci_address,
+	};
+	struct modem_cmux_msc_signals signals = {
+		.ea = 1,
+		.fc = dlci->rx_full ? 1 : 0,
+		.rtc = dlci->state == MODEM_CMUX_DLCI_STATE_OPEN ? 1 : 0,
+		.rtr = dlci->state == MODEM_CMUX_DLCI_STATE_OPEN ? 1 : 0,
+		.dv = 1,
+	};
+	struct modem_cmux_command_msc cmd = {
+		.command = {
+			.type = {
+				.ea = 1,
+				.cr = 1,
+				.value = MODEM_CMUX_COMMAND_MSC,
+			},
+			.length = {
+				.ea = 1,
+				.value = sizeof(cmd.value),
+			},
+		},
+		.value[0] = modem_cmux_msc_addr_encode(addr),
+		.value[1] = modem_cmux_msc_signals_encode(signals),
+	};
+
+	struct modem_cmux_frame frame = {
+		.dlci_address = 0,
+		.cr = cmux->initiator,
+		.pf = false,
+		.type = MODEM_CMUX_FRAME_TYPE_UIH,
+		.data = (void *)&cmd,
+		.data_len = sizeof(cmd),
+	};
+
+	LOG_DBG("Sending MSC command for DLCI %u, FC:%d RTR: %d DV: %d", addr.dlci_address,
+		signals.fc, signals.rtr, signals.dv);
+	modem_cmux_transmit_cmd_frame(cmux, &frame);
+}
+
 static void modem_cmux_on_msc_command(struct modem_cmux *cmux, struct modem_cmux_command *command)
 {
-	if (command->type.cr) {
-		modem_cmux_acknowledge_received_frame(cmux);
+	if (!command->type.cr) {
+		return;
+	}
+
+	modem_cmux_acknowledge_received_frame(cmux);
+
+	uint8_t len = command->length.value;
+
+	if (len != 2 && len != 3) {
+		LOG_WRN("Unexpected MSC command length %d", (int)len);
+		return;
+	}
+
+	struct modem_cmux_msc_addr msc = modem_cmux_msc_addr_decode(command->value[0]);
+	struct modem_cmux_msc_signals signals = modem_cmux_msc_signals_decode(command->value[1]);
+	struct modem_cmux_dlci *dlci = modem_cmux_find_dlci(cmux, msc.dlci_address);
+
+	if (dlci) {
+		LOG_DBG("MSC command received for DLCI %u", msc.dlci_address);
+		bool fc_signal = signals.fc || !signals.rtr;
+
+		if (fc_signal != dlci->flow_control) {
+			if (fc_signal) {
+				dlci->flow_control = true;
+				LOG_DBG("DLCI %u flow control ON", dlci->dlci_address);
+			} else {
+				dlci->flow_control = false;
+				LOG_DBG("DLCI %u flow control OFF", dlci->dlci_address);
+				modem_pipe_notify_transmit_idle(&dlci->pipe);
+			}
+		}
+		/* As we have received MSC, send also our MSC */
+		if (!dlci->msc_sent && dlci->state == MODEM_CMUX_DLCI_STATE_OPEN) {
+			dlci->msc_sent = true;
+			modem_cmux_send_msc(cmux, dlci);
+		}
 	}
 }
 
@@ -439,6 +591,7 @@ static void modem_cmux_on_fcon_command(struct modem_cmux *cmux)
 	cmux->flow_control_on = true;
 	k_mutex_unlock(&cmux->transmit_rb_lock);
 	modem_cmux_acknowledge_received_frame(cmux);
+	modem_cmux_dlci_notify_transmit_idle(cmux);
 }
 
 static void modem_cmux_on_fcoff_command(struct modem_cmux *cmux)
@@ -670,15 +823,15 @@ static void modem_cmux_on_control_frame(struct modem_cmux *cmux)
 	}
 }
 
-static struct modem_cmux_dlci *modem_cmux_find_dlci(struct modem_cmux *cmux)
+static struct modem_cmux_dlci *modem_cmux_find_dlci(struct modem_cmux *cmux, uint8_t dlci_address)
 {
 	sys_snode_t *node;
 	struct modem_cmux_dlci *dlci;
 
 	SYS_SLIST_FOR_EACH_NODE(&cmux->dlcis, node) {
 		dlci = (struct modem_cmux_dlci *)node;
 
-		if (dlci->dlci_address == cmux->frame.dlci_address) {
+		if (dlci->dlci_address == dlci_address) {
 			return dlci;
 		}
 	}
@@ -710,6 +863,12 @@ static void modem_cmux_on_dlci_frame_ua(struct modem_cmux_dlci *dlci)
 		k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER);
 		ring_buf_reset(&dlci->receive_rb);
 		k_mutex_unlock(&dlci->receive_rb_lock);
+		if (dlci->cmux->initiator) {
+			modem_cmux_send_msc(dlci->cmux, dlci);
+			dlci->msc_sent = true;
+		} else {
+			dlci->msc_sent = false;
+		}
 		break;
 
 	case MODEM_CMUX_DLCI_STATE_CLOSING:
@@ -742,6 +901,12 @@ static void modem_cmux_on_dlci_frame_uih(struct modem_cmux_dlci *dlci)
 		LOG_WRN("DLCI %u receive buffer overrun (dropped %u out of %u bytes)",
 			dlci->dlci_address, cmux->frame.data_len - written, cmux->frame.data_len);
 	}
+	if (written < cmux->frame.data_len ||
+	    ring_buf_space_get(&dlci->receive_rb) < MODEM_CMUX_DATA_FRAME_SIZE_MAX) {
+		LOG_WRN("DLCI %u receive buffer is full", dlci->dlci_address);
+		dlci->rx_full = true;
+		modem_cmux_send_msc(cmux, dlci);
+	}
 	modem_pipe_notify_receive_ready(&dlci->pipe);
 }
 
@@ -758,6 +923,7 @@ static void modem_cmux_on_dlci_frame_sabm(struct modem_cmux_dlci *dlci)
 
 	LOG_DBG("DLCI %u SABM request accepted, DLCI opened", dlci->dlci_address);
 	dlci->state = MODEM_CMUX_DLCI_STATE_OPEN;
+	dlci->msc_sent = false;
 	modem_pipe_notify_opened(&dlci->pipe);
 	k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER);
 	ring_buf_reset(&dlci->receive_rb);
@@ -791,7 +957,7 @@ static void modem_cmux_on_dlci_frame(struct modem_cmux *cmux)
 		return;
 	}
 
-	dlci = modem_cmux_find_dlci(cmux);
+	dlci = modem_cmux_find_dlci(cmux, cmux->frame.dlci_address);
 	if (dlci == NULL) {
 		LOG_WRN("Frame intended for unconfigured DLCI %u.",
 			cmux->frame.dlci_address);
@@ -1066,7 +1232,9 @@ static void modem_cmux_dlci_notify_transmit_idle(struct modem_cmux *cmux)
 
 	SYS_SLIST_FOR_EACH_NODE(&cmux->dlcis, node) {
 		dlci = (struct modem_cmux_dlci *)node;
-		modem_pipe_notify_transmit_idle(&dlci->pipe);
+		if (!dlci->flow_control) {
+			modem_pipe_notify_transmit_idle(&dlci->pipe);
+		}
 	}
 }
 
@@ -1241,6 +1409,10 @@ static int modem_cmux_dlci_pipe_api_transmit(void *data, const uint8_t *buf, siz
 	struct modem_cmux *cmux = dlci->cmux;
 	int ret = 0;
 
+	if (dlci->flow_control) {
+		return 0;
+	}
+
 	K_SPINLOCK(&cmux->work_lock) {
 		if (!cmux->attached) {
 			ret = -EPERM;
@@ -1275,6 +1447,15 @@ static int modem_cmux_dlci_pipe_api_receive(void *data, uint8_t *buf, size_t siz
 
 	ret = ring_buf_get(&dlci->receive_rb, buf, size);
 	k_mutex_unlock(&dlci->receive_rb_lock);
+
+	/* Release FC if set */
+	if (dlci->rx_full &&
+	    ring_buf_space_get(&dlci->receive_rb) >= MODEM_CMUX_DATA_FRAME_SIZE_MAX) {
+		LOG_DBG("DLCI %u receive buffer is no longer full", dlci->dlci_address);
+		dlci->rx_full = false;
+		modem_cmux_send_msc(dlci->cmux, dlci);
+	}
+
 	return ret;
 }
 
@@ -1321,6 +1502,7 @@ static void modem_cmux_dlci_open_handler(struct k_work *item)
 	dlci = CONTAINER_OF(dwork, struct modem_cmux_dlci, open_work);
 
 	dlci->state = MODEM_CMUX_DLCI_STATE_OPENING;
+	dlci->msc_sent = false;
 
 	struct modem_cmux_frame frame = {
 		.dlci_address = dlci->dlci_address,

tests/subsys/modem/mock/modem_backend_mock.c

@@ -52,12 +52,15 @@ static int modem_backend_mock_transmit(void *data, const uint8_t *buf, size_t si
 		return ret;
 	}
 
-	ret = ring_buf_put(&mock->tx_rb, buf, size);
 	if (modem_backend_mock_update(mock, buf, size)) {
+		/* Skip ringbuffer if transaction consumes bytes */
+		ret = size;
 		modem_backend_mock_put(mock, mock->transaction->put,
 				       mock->transaction->put_size);
 
-		mock->transaction = NULL;
+		modem_backend_mock_prime(mock, mock->transaction->next);
+	} else {
+		ret = ring_buf_put(&mock->tx_rb, buf, size);
 	}
 
 	k_work_submit(&mock->transmit_idle_work);
@@ -137,6 +140,10 @@ int modem_backend_mock_get(struct modem_backend_mock *mock, uint8_t *buf, size_t
 
 void modem_backend_mock_put(struct modem_backend_mock *mock, const uint8_t *buf, size_t size)
 {
+	if (size == 0) {
+		return;
+	}
+
 	__ASSERT(ring_buf_put(&mock->rx_rb, buf, size) == size,
 		 "Mock buffer capacity exceeded");
 
@@ -155,3 +162,10 @@ void modem_backend_mock_bridge(struct modem_backend_mock *mock_a, struct modem_b
 	mock_a->bridge = mock_b;
 	mock_b->bridge = mock_a;
 }
+
+void modem_backend_mock_wait_for_transaction(struct modem_backend_mock *mock)
+{
+	while (mock->transaction) {
+		k_msleep(1);
+	}
+}

tests/subsys/modem/mock/modem_backend_mock.h

@@ -19,6 +19,9 @@ struct modem_backend_mock_transaction {
 	/* Data which will be put in response to get data */
 	const uint8_t *put;
 	size_t put_size;
+
+	/* Next transaction in chain */
+	const struct modem_backend_mock_transaction *next;
 };
 
 struct modem_backend_mock {
@@ -62,4 +65,6 @@ void modem_backend_mock_prime(struct modem_backend_mock *mock,
 void modem_backend_mock_bridge(struct modem_backend_mock *mock_a,
 			       struct modem_backend_mock *mock_b);
 
+void modem_backend_mock_wait_for_transaction(struct modem_backend_mock *mock);
+
 #endif /* ZEPHYR_DRIVERS_MODEM_MODEM_PIPE_MOCK */