[nrf fromlist] modem: cmux: Implement Power Saving Control message

Signal powersaving mode for the remote end using PSC command.
Wakes up the remote end from powersaving mode by sending flag characters.

This method is defined in 3GPP TS 27.010.
Sections 5.4.6.3.2 Power Saving Control (PSC) and
5.4.7 Power Control and Wake-up Mechanisms.

Essentially it is one PSC command to indicate a sleep state, and
then repeated flag characters to wake up the remote end or indicate
that we have been woken up.

Upstream PR #: 97362

Signed-off-by: Seppo Takalo <[email protected]>

Author: SeppoTakalo

include/zephyr/modem/cmux.h

@@ -72,6 +72,10 @@ enum modem_cmux_state {
 	MODEM_CMUX_STATE_DISCONNECTED = 0,
 	MODEM_CMUX_STATE_CONNECTING,
 	MODEM_CMUX_STATE_CONNECTED,
+	MODEM_CMUX_STATE_ENTER_POWERSAVE,
+	MODEM_CMUX_STATE_POWERSAVE,
+	MODEM_CMUX_STATE_CONFIRM_POWERSAVE,
+	MODEM_CMUX_STATE_WAKEUP,
 	MODEM_CMUX_STATE_DISCONNECTING,
 };
 
@@ -187,6 +191,7 @@ struct modem_cmux {
 
 	/* Synchronize actions */
 	struct k_event event;
+	k_timepoint_t t3_timepoint;
 
 	/* Statistics */
 #if CONFIG_MODEM_STATS

subsys/modem/Kconfig

@@ -64,6 +64,14 @@ config MODEM_CMUX_WORK_BUFFER_SIZE_EXTRA
 	  Extra bytes to add to the work buffers used by the CMUX module.
 	  The default size of these buffers is MODEM_CMUX_MTU + 7 (CMUX header size).
 
+config MODEM_CMUX_T3_TIMEOUT
+	int "CMUX T3 timeout in seconds"
+	range 1 255
+	default 10
+	help
+	  Response Timer for wake-up procedure(T3).
+	  Time in seconds before the link is considered dead.
+
 module = MODEM_CMUX
 module-str = modem_cmux
 source "subsys/logging/Kconfig.template.log_config"

subsys/modem/modem_cmux.c

@@ -37,9 +37,7 @@ LOG_MODULE_REGISTER(modem_cmux, CONFIG_MODEM_CMUX_LOG_LEVEL);
 
 #define MODEM_CMUX_T1_TIMEOUT			(K_MSEC(330))
 #define MODEM_CMUX_T2_TIMEOUT			(K_MSEC(660))
-
-#define MODEM_CMUX_EVENT_CONNECTED_BIT		(BIT(0))
-#define MODEM_CMUX_EVENT_DISCONNECTED_BIT	(BIT(1))
+#define MODEM_CMUX_T3_TIMEOUT                   (K_SECONDS(CONFIG_MODEM_CMUX_T3_TIMEOUT))
 
 enum modem_cmux_frame_types {
 	MODEM_CMUX_FRAME_TYPE_RR = 0x01,
@@ -101,6 +99,7 @@ struct modem_cmux_msc_addr {
 
 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 void modem_cmux_tx_bypass(struct modem_cmux *cmux, const uint8_t *data, size_t len);
 
 static void set_state(struct modem_cmux *cmux, enum modem_cmux_state state)
 {
@@ -118,6 +117,22 @@ static bool is_connected(struct modem_cmux *cmux)
 	return cmux->state == MODEM_CMUX_STATE_CONNECTED;
 }
 
+static bool is_powersaving(struct modem_cmux *cmux)
+{
+	return cmux->state == MODEM_CMUX_STATE_POWERSAVE;
+}
+
+static bool is_waking_up(struct modem_cmux *cmux)
+{
+	return cmux->state == MODEM_CMUX_STATE_WAKEUP;
+}
+
+static bool is_transitioning_to_powersave(struct modem_cmux *cmux)
+{
+	return (cmux->state == MODEM_CMUX_STATE_ENTER_POWERSAVE ||
+		cmux->state == MODEM_CMUX_STATE_CONFIRM_POWERSAVE);
+}
+
 static bool modem_cmux_command_type_is_valid(const struct modem_cmux_command_type type)
 {
 	/* All commands are only 7 bits, so EA is always set */
@@ -581,7 +596,7 @@ static int16_t modem_cmux_transmit_data_frame(struct modem_cmux *cmux,
 
 	k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
 
-	if (cmux->flow_control_on == false) {
+	if (cmux->flow_control_on == false || is_transitioning_to_powersave(cmux)) {
 		k_mutex_unlock(&cmux->transmit_rb_lock);
 		return 0;
 	}
@@ -630,6 +645,34 @@ static void modem_cmux_acknowledge_received_frame(struct modem_cmux *cmux)
 	}
 }
 
+static void modem_cmux_send_psc(struct modem_cmux *cmux)
+{
+	uint16_t len;
+	uint8_t *data = modem_cmux_command_encode(&(struct modem_cmux_command){
+		.type.ea = 1,
+		.type.cr = 1,
+		.type.value = MODEM_CMUX_COMMAND_PSC,
+		.length.ea = 1,
+		.length.value = 0,
+	}, &len);
+
+	if (data == NULL) {
+		return;
+	}
+
+	struct modem_cmux_frame frame = {
+		.dlci_address = 0,
+		.cr = cmux->initiator,
+		.pf = true,
+		.type = MODEM_CMUX_FRAME_TYPE_UIH,
+		.data = data,
+		.data_len = len,
+	};
+
+	LOG_DBG("Sending PSC command");
+	modem_cmux_transmit_cmd_frame(cmux, &frame);
+}
+
 static void modem_cmux_send_msc(struct modem_cmux *cmux, struct modem_cmux_dlci *dlci)
 {
 	if (cmux == NULL || dlci == NULL) {
@@ -827,12 +870,28 @@ static void modem_cmux_respond_unsupported_cmd(struct modem_cmux *cmux)
 	modem_cmux_transmit_cmd_frame(cmux, &frame);
 }
 
+static void modem_cmux_on_psc_command(struct modem_cmux *cmux)
+{
+	LOG_DBG("Received power saving command");
+	k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
+	set_state(cmux, MODEM_CMUX_STATE_CONFIRM_POWERSAVE);
+	modem_cmux_acknowledge_received_frame(cmux);
+	k_mutex_unlock(&cmux->transmit_rb_lock);
+}
+
+static void modem_cmux_on_psc_response(struct modem_cmux *cmux)
+{
+	LOG_DBG("Enter power saving");
+	k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);
+	set_state(cmux, MODEM_CMUX_STATE_POWERSAVE);
+	k_mutex_unlock(&cmux->transmit_rb_lock);
+}
+
 static void modem_cmux_on_control_frame_uih(struct modem_cmux *cmux)
 {
 	struct modem_cmux_command command;
 
-	if ((cmux->state != MODEM_CMUX_STATE_CONNECTED) &&
-	    (cmux->state != MODEM_CMUX_STATE_DISCONNECTING)) {
+	if (cmux->state < MODEM_CMUX_STATE_CONNECTED) {
 		LOG_DBG("Unexpected UIH frame");
 		return;
 	}
@@ -851,6 +910,9 @@ static void modem_cmux_on_control_frame_uih(struct modem_cmux *cmux)
 		case MODEM_CMUX_COMMAND_CLD:
 			modem_cmux_on_cld_command(cmux, &command);
 			break;
+		case MODEM_CMUX_COMMAND_PSC:
+			modem_cmux_on_psc_response(cmux);
+			break;
 		default:
 			/* Responses to other commands are ignored */
 			break;
@@ -875,6 +937,10 @@ static void modem_cmux_on_control_frame_uih(struct modem_cmux *cmux)
 		modem_cmux_on_fcoff_command(cmux);
 		break;
 
+	case MODEM_CMUX_COMMAND_PSC:
+		modem_cmux_on_psc_command(cmux);
+		break;
+
 	default:
 		LOG_DBG("Unknown control command");
 		modem_cmux_respond_unsupported_cmd(cmux);
@@ -1140,6 +1206,11 @@ static void modem_cmux_on_frame(struct modem_cmux *cmux)
 	modem_cmux_advertise_receive_buf_stats(cmux);
 #endif
 
+	if (is_powersaving(cmux) || is_waking_up(cmux)) {
+		set_state(cmux, MODEM_CMUX_STATE_CONNECTED);
+		LOG_DBG("Exit powersave on received frame");
+	}
+
 	if (cmux->frame.dlci_address == 0) {
 		modem_cmux_on_control_frame(cmux);
 	} else {
@@ -1170,8 +1241,10 @@ static void modem_cmux_process_received_byte(struct modem_cmux *cmux, uint8_t by
 
 	switch (cmux->receive_state) {
 	case MODEM_CMUX_RECEIVE_STATE_SOF:
+		cmux->frame_header_len = 0;
 		if (byte == MODEM_CMUX_SOF) {
 			cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_RESYNC;
+			cmux->frame_header_len++;
 			break;
 		}
 
@@ -1183,6 +1256,20 @@ static void modem_cmux_process_received_byte(struct modem_cmux *cmux, uint8_t by
 		 * 0xF9 could also be a valid address field for DLCI 62.
 		 */
 		if (byte == MODEM_CMUX_SOF) {
+			/* Use "header_len" to count SOF bytes, only start transmitting
+			 * flag bytes after receiving more than 3 flags.
+			 * Don't reply flags if we are transitioning between modes or
+			 * if T3 timer is still active (suppress residual flags).
+			 */
+			cmux->frame_header_len++;
+			if ((is_powersaving(cmux) ||
+			     (is_connected(cmux) && sys_timepoint_expired(cmux->t3_timepoint))) &&
+			    cmux->frame_header_len > 3) {
+				modem_cmux_tx_bypass(cmux, &(char){MODEM_CMUX_SOF}, 1);
+			}
+			if (is_waking_up(cmux)) {
+				k_work_reschedule(&cmux->transmit_work, K_NO_WAIT);
+			}
 			break;
 		}
 
@@ -1351,21 +1438,15 @@ static void modem_cmux_receive_handler(struct k_work *item)
 	int ret;
 
 	/* Receive data from pipe */
-	ret = modem_pipe_receive(cmux->pipe, cmux->work_buf, sizeof(cmux->work_buf));
-	if (ret < 1) {
-		if (ret < 0) {
-			LOG_ERR("Pipe receiving error: %d", ret);
+	while ((ret = modem_pipe_receive(cmux->pipe, cmux->work_buf, sizeof(cmux->work_buf))) > 0) {
+		/* Process received data */
+		for (int i = 0; i < ret; i++) {
+			modem_cmux_process_received_byte(cmux, cmux->work_buf[i]);
 		}
-		return;
 	}
-
-	/* Process received data */
-	for (int i = 0; i < ret; i++) {
-		modem_cmux_process_received_byte(cmux, cmux->work_buf[i]);
+	if (ret < 0) {
+		LOG_ERR("Pipe receiving error: %d", ret);
 	}
-
-	/* Reschedule received work */
-	modem_work_schedule(&cmux->receive_work, K_NO_WAIT);
 }
 
 static void modem_cmux_dlci_notify_transmit_idle(struct modem_cmux *cmux)
@@ -1381,6 +1462,51 @@ static void modem_cmux_dlci_notify_transmit_idle(struct modem_cmux *cmux)
 	}
 }
 
+/** Transmit bytes bypassing the CMUX buffers.
+ * Causes modem_cmux_transmit_handler() to be rescheduled as a result of TRANSMIT_IDLE event.
+ */
+static void modem_cmux_tx_bypass(struct modem_cmux *cmux, const uint8_t *data, size_t len)
+{
+	if (cmux == NULL) {
+		return;
+	}
+
+	modem_pipe_transmit(cmux->pipe, data, len);
+}
+
+static bool powersave_wait_wakeup(struct modem_cmux *cmux)
+{
+	static const uint8_t wakeup_pattern[] = {MODEM_CMUX_SOF, MODEM_CMUX_SOF, MODEM_CMUX_SOF,
+						 MODEM_CMUX_SOF, MODEM_CMUX_SOF};
+	int ret;
+
+	if (is_powersaving(cmux)) {
+		LOG_DBG("Power saving mode, wake up first");
+		set_state(cmux, MODEM_CMUX_STATE_WAKEUP);
+		cmux->t3_timepoint = sys_timepoint_calc(MODEM_CMUX_T3_TIMEOUT);
+		modem_cmux_tx_bypass(cmux, wakeup_pattern, sizeof(wakeup_pattern));
+		return true;
+	}
+
+	if (is_waking_up(cmux)) {
+		if (sys_timepoint_expired(cmux->t3_timepoint)) {
+			LOG_ERR("Wake up timed out, link dead");
+			set_state(cmux, MODEM_CMUX_STATE_DISCONNECTED);
+			modem_cmux_raise_event(cmux, MODEM_CMUX_EVENT_DISCONNECTED);
+			return true;
+		}
+		if (cmux->receive_state != MODEM_CMUX_RECEIVE_STATE_RESYNC) {
+			/* Retry single flag, until remote wakes up */
+			modem_cmux_tx_bypass(cmux, &(uint8_t){MODEM_CMUX_SOF}, 1);
+			return true;
+		}
+		set_state(cmux, MODEM_CMUX_STATE_CONNECTED);
+		LOG_DBG("Woke up from power saving mode");
+	}
+
+	return false;
+}
+
 static void modem_cmux_transmit_handler(struct k_work *item)
 {
 	struct k_work_delayable *dwork = k_work_delayable_from_work(item);
@@ -1403,6 +1529,11 @@ static void modem_cmux_transmit_handler(struct k_work *item)
 			break;
 		}
 
+		if (powersave_wait_wakeup(cmux)) {
+			k_mutex_unlock(&cmux->transmit_rb_lock);
+			return;
+		}
+
 		reserved_size = ring_buf_get_claim(&cmux->transmit_rb, &reserved, UINT32_MAX);
 
 		ret = modem_pipe_transmit(cmux->pipe, reserved, reserved_size);
@@ -1423,11 +1554,14 @@ static void modem_cmux_transmit_handler(struct k_work *item)
 		}
 	}
 
-	k_mutex_unlock(&cmux->transmit_rb_lock);
-
 	if (transmit_rb_empty) {
+		if (cmux->state == MODEM_CMUX_STATE_CONFIRM_POWERSAVE) {
+			set_state(cmux, MODEM_CMUX_STATE_POWERSAVE);
+			LOG_DBG("Entered power saving mode");
+		}
 		modem_cmux_dlci_notify_transmit_idle(cmux);
 	}
+	k_mutex_unlock(&cmux->transmit_rb_lock);
 }
 
 static void modem_cmux_connect_handler(struct k_work *item)
@@ -1724,6 +1858,7 @@ void modem_cmux_init(struct modem_cmux *cmux, const struct modem_cmux_config *co
 	cmux->user_data = config->user_data;
 	cmux->receive_buf = config->receive_buf;
 	cmux->receive_buf_size = config->receive_buf_size;
+	cmux->t3_timepoint = sys_timepoint_calc(K_NO_WAIT);
 	sys_slist_init(&cmux->dlcis);
 	ring_buf_init(&cmux->transmit_rb, config->transmit_buf_size, config->transmit_buf);
 	k_mutex_init(&cmux->transmit_rb_lock);