modem: cmux: Implement DM and NSC responses

Implement Disconnect Mode(DM) and
Non-Supported Command(NSC) responses.

Close DLC when receiving DM response.

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

Author: SeppoTakalo

subsys/modem/modem_cmux.c

@@ -465,6 +465,43 @@ static void modem_cmux_on_control_frame_ua(struct modem_cmux *cmux)
 	k_event_post(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT);
 }
 
+static void modem_cmux_respond_unsupported_cmd(struct modem_cmux *cmux)
+{
+	struct modem_cmux_frame frame;
+	struct modem_cmux_command *cmd;
+
+	memcpy(&frame, &cmux->frame, sizeof(cmux->frame));
+	if (modem_cmux_wrap_command(&cmd, frame.data, frame.data_len) < 0) {
+		LOG_WRN("Invalid command");
+		return;
+	}
+
+	struct {
+		struct modem_cmux_command nsc;
+		struct modem_cmux_command_type value;
+	} nsc_cmd = {
+		.nsc = {
+			.type = {
+				.ea = 1,
+				.cr = 0,
+				.value = MODEM_CMUX_COMMAND_NSC,
+			},
+			.length = {
+				.ea = 1,
+				.value = 1,
+			},
+		},
+		.value = cmd->type,
+	};
+
+	frame.data = (uint8_t *)&nsc_cmd;
+	frame.data_len = sizeof(nsc_cmd);
+
+	if (modem_cmux_transmit_cmd_frame(cmux, &frame) == false) {
+		LOG_WRN("CMD response buffer overflow");
+	}
+}
+
 static void modem_cmux_on_control_frame_uih(struct modem_cmux *cmux)
 {
 	struct modem_cmux_command *command;
@@ -501,6 +538,7 @@ static void modem_cmux_on_control_frame_uih(struct modem_cmux *cmux)
 
 	default:
 		LOG_DBG("Unknown control command");
+		modem_cmux_respond_unsupported_cmd(cmux);
 		break;
 	}
 }
@@ -524,6 +562,25 @@ static void modem_cmux_connect_response_transmit(struct modem_cmux *cmux)
 	modem_cmux_transmit_cmd_frame(cmux, &frame);
 }
 
+static void modem_cmux_dm_response_transmit(struct modem_cmux *cmux)
+{
+	if (cmux == NULL) {
+		return;
+	}
+
+	struct modem_cmux_frame frame = {
+		.dlci_address = cmux->frame.dlci_address,
+		.cr = cmux->frame.cr,
+		.pf = 1,
+		.type = MODEM_CMUX_FRAME_TYPE_DM,
+		.data = NULL,
+		.data_len = 0,
+	};
+
+	LOG_DBG("Send DM response");
+	modem_cmux_transmit_cmd_frame(cmux, &frame);
+}
+
 static void modem_cmux_on_control_frame_sabm(struct modem_cmux *cmux)
 {
 	modem_cmux_connect_response_transmit(cmux);
@@ -582,6 +639,13 @@ static struct modem_cmux_dlci *modem_cmux_find_dlci(struct modem_cmux *cmux)
 	return NULL;
 }
 
+static void modem_cmux_on_dlci_frame_dm(struct modem_cmux_dlci *dlci)
+{
+	dlci->state = MODEM_CMUX_DLCI_STATE_CLOSED;
+	modem_pipe_notify_closed(&dlci->pipe);
+	k_work_cancel_delayable(&dlci->close_work);
+}
+
 static void modem_cmux_on_dlci_frame_ua(struct modem_cmux_dlci *dlci)
 {
 	switch (dlci->state) {
@@ -595,9 +659,8 @@ static void modem_cmux_on_dlci_frame_ua(struct modem_cmux_dlci *dlci)
 		break;
 
 	case MODEM_CMUX_DLCI_STATE_CLOSING:
-		dlci->state = MODEM_CMUX_DLCI_STATE_CLOSED;
-		modem_pipe_notify_closed(&dlci->pipe);
-		k_work_cancel_delayable(&dlci->close_work);
+		/* Closing is handled same way in DM or as UA reply to DISC */
+		modem_cmux_on_dlci_frame_dm(dlci);
 		break;
 
 	default:
@@ -606,6 +669,8 @@ static void modem_cmux_on_dlci_frame_ua(struct modem_cmux_dlci *dlci)
 	}
 }
 
+
+
 static void modem_cmux_on_dlci_frame_uih(struct modem_cmux_dlci *dlci)
 {
 	struct modem_cmux *cmux = dlci->cmux;
@@ -651,7 +716,7 @@ static void modem_cmux_on_dlci_frame_disc(struct modem_cmux_dlci *dlci)
 	modem_cmux_connect_response_transmit(cmux);
 
 	if (dlci->state != MODEM_CMUX_DLCI_STATE_OPEN) {
-		LOG_DBG("Unexpected Disc frame");
+		modem_cmux_dm_response_transmit(cmux);
 		return;
 	}
 
@@ -667,8 +732,9 @@ static void modem_cmux_on_dlci_frame(struct modem_cmux *cmux)
 
 	dlci = modem_cmux_find_dlci(cmux);
 	if (dlci == NULL) {
-		LOG_WRN("Ignoring frame intended for unconfigured DLCI %u.",
+		LOG_WRN("Frame intended for unconfigured DLCI %u.",
 			cmux->frame.dlci_address);
+		modem_cmux_dm_response_transmit(cmux);
 		return;
 	}
 
@@ -688,7 +754,9 @@ static void modem_cmux_on_dlci_frame(struct modem_cmux *cmux)
 	case MODEM_CMUX_FRAME_TYPE_DISC:
 		modem_cmux_on_dlci_frame_disc(dlci);
 		break;
-
+	case MODEM_CMUX_FRAME_TYPE_DM:
+		modem_cmux_on_dlci_frame_dm(dlci);
+		break;
 	default:
 		LOG_WRN("Unknown %s frame type", "DLCI");
 		break;