/* * Copyright (c) 2022 Trackunit Corporation * * SPDX-License-Identifier: Apache-2.0 */ #include LOG_MODULE_REGISTER(modem_cmux, CONFIG_MODEM_CMUX_LOG_LEVEL); #include #include #include #include #define MODEM_CMUX_FCS_POLYNOMIAL (0xE0) #define MODEM_CMUX_FCS_INIT_VALUE (0xFF) #define MODEM_CMUX_EA (0x01) #define MODEM_CMUX_CR (0x02) #define MODEM_CMUX_PF (0x10) #define MODEM_CMUX_FRAME_SIZE_MAX (0x08) #define MODEM_CMUX_DATA_SIZE_MIN (0x08) #define MODEM_CMUX_DATA_FRAME_SIZE_MIN (MODEM_CMUX_FRAME_SIZE_MAX + \ MODEM_CMUX_DATA_SIZE_MIN) #define MODEM_CMUX_CMD_DATA_SIZE_MAX (0x08) #define MODEM_CMUX_CMD_FRAME_SIZE_MAX (MODEM_CMUX_FRAME_SIZE_MAX + \ MODEM_CMUX_CMD_DATA_SIZE_MAX) #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)) enum modem_cmux_frame_types { MODEM_CMUX_FRAME_TYPE_RR = 0x01, MODEM_CMUX_FRAME_TYPE_UI = 0x03, MODEM_CMUX_FRAME_TYPE_RNR = 0x05, MODEM_CMUX_FRAME_TYPE_REJ = 0x09, MODEM_CMUX_FRAME_TYPE_DM = 0x0F, MODEM_CMUX_FRAME_TYPE_SABM = 0x2F, MODEM_CMUX_FRAME_TYPE_DISC = 0x43, MODEM_CMUX_FRAME_TYPE_UA = 0x63, MODEM_CMUX_FRAME_TYPE_UIH = 0xEF, }; enum modem_cmux_command_types { MODEM_CMUX_COMMAND_NSC = 0x04, MODEM_CMUX_COMMAND_TEST = 0x08, MODEM_CMUX_COMMAND_PSC = 0x10, MODEM_CMUX_COMMAND_RLS = 0x14, MODEM_CMUX_COMMAND_FCOFF = 0x18, MODEM_CMUX_COMMAND_PN = 0x20, MODEM_CMUX_COMMAND_RPN = 0x24, MODEM_CMUX_COMMAND_FCON = 0x28, MODEM_CMUX_COMMAND_CLD = 0x30, MODEM_CMUX_COMMAND_SNC = 0x34, MODEM_CMUX_COMMAND_MSC = 0x38, }; struct modem_cmux_command_type { uint8_t ea: 1; uint8_t cr: 1; uint8_t value: 6; }; struct modem_cmux_command_length { uint8_t ea: 1; uint8_t value: 7; }; struct modem_cmux_command { struct modem_cmux_command_type type; struct modem_cmux_command_length length; uint8_t value[]; }; static int modem_cmux_wrap_command(struct modem_cmux_command **command, const uint8_t *data, uint16_t data_len) { if ((data == NULL) || (data_len < 2)) { return -EINVAL; } (*command) = (struct modem_cmux_command *)data; if (((*command)->length.ea == 0) || ((*command)->type.ea == 0)) { return -EINVAL; } if ((*command)->length.value != (data_len - 2)) { return -EINVAL; } return 0; } static struct modem_cmux_command *modem_cmux_command_wrap(const uint8_t *data) { return (struct modem_cmux_command *)data; } static const char *modem_cmux_frame_type_to_str(enum modem_cmux_frame_types frame_type) { switch (frame_type) { case MODEM_CMUX_FRAME_TYPE_RR: return "RR"; case MODEM_CMUX_FRAME_TYPE_UI: return "UI"; case MODEM_CMUX_FRAME_TYPE_RNR: return "RNR"; case MODEM_CMUX_FRAME_TYPE_REJ: return "REJ"; case MODEM_CMUX_FRAME_TYPE_DM: return "DM"; case MODEM_CMUX_FRAME_TYPE_SABM: return "SABM"; case MODEM_CMUX_FRAME_TYPE_DISC: return "DISC"; case MODEM_CMUX_FRAME_TYPE_UA: return "UA"; case MODEM_CMUX_FRAME_TYPE_UIH: return "UIH"; } return ""; } static void modem_cmux_log_frame(const struct modem_cmux_frame *frame, const char *action, size_t hexdump_len) { LOG_DBG("%s ch:%u cr:%u pf:%u type:%s dlen:%u", action, frame->dlci_address, frame->cr, frame->pf, modem_cmux_frame_type_to_str(frame->type), frame->data_len); LOG_HEXDUMP_DBG(frame->data, hexdump_len, "data:"); } static void modem_cmux_log_transmit_frame(const struct modem_cmux_frame *frame) { modem_cmux_log_frame(frame, "tx", frame->data_len); } static void modem_cmux_log_received_frame(const struct modem_cmux_frame *frame) { modem_cmux_log_frame(frame, "rcvd", frame->data_len); } #if CONFIG_MODEM_STATS static uint32_t modem_cmux_get_receive_buf_length(struct modem_cmux *cmux) { return cmux->receive_buf_len; } static uint32_t modem_cmux_get_receive_buf_size(struct modem_cmux *cmux) { return cmux->receive_buf_size; } static uint32_t modem_cmux_get_transmit_buf_length(struct modem_cmux *cmux) { return ring_buf_size_get(&cmux->transmit_rb); } static uint32_t modem_cmux_get_transmit_buf_size(struct modem_cmux *cmux) { return ring_buf_capacity_get(&cmux->transmit_rb); } static void modem_cmux_init_buf_stats(struct modem_cmux *cmux) { uint32_t size; size = modem_cmux_get_receive_buf_size(cmux); modem_stats_buffer_init(&cmux->receive_buf_stats, "cmux_rx", size); size = modem_cmux_get_transmit_buf_size(cmux); modem_stats_buffer_init(&cmux->transmit_buf_stats, "cmux_tx", size); } static void modem_cmux_advertise_transmit_buf_stats(struct modem_cmux *cmux) { uint32_t length; length = modem_cmux_get_transmit_buf_length(cmux); modem_stats_buffer_advertise_length(&cmux->transmit_buf_stats, length); } static void modem_cmux_advertise_receive_buf_stats(struct modem_cmux *cmux) { uint32_t length; length = modem_cmux_get_receive_buf_length(cmux); modem_stats_buffer_advertise_length(&cmux->receive_buf_stats, length); } #endif static const char *modem_cmux_command_type_to_str(enum modem_cmux_command_types command_type) { switch (command_type) { case MODEM_CMUX_COMMAND_NSC: return "NSC"; case MODEM_CMUX_COMMAND_TEST: return "TEST"; case MODEM_CMUX_COMMAND_PSC: return "PSC"; case MODEM_CMUX_COMMAND_RLS: return "RLS"; case MODEM_CMUX_COMMAND_FCOFF: return "FCOFF"; case MODEM_CMUX_COMMAND_PN: return "PN"; case MODEM_CMUX_COMMAND_RPN: return "RPN"; case MODEM_CMUX_COMMAND_FCON: return "FCON"; case MODEM_CMUX_COMMAND_CLD: return "CLD"; case MODEM_CMUX_COMMAND_SNC: return "SNC"; case MODEM_CMUX_COMMAND_MSC: return "MSC"; } return ""; } static void modem_cmux_log_transmit_command(const struct modem_cmux_command *command) { LOG_DBG("ea:%u,cr:%u,type:%s", command->type.ea, command->type.cr, modem_cmux_command_type_to_str(command->type.value)); LOG_HEXDUMP_DBG(command->value, command->length.value, "data:"); } static void modem_cmux_log_received_command(const struct modem_cmux_command *command) { LOG_DBG("ea:%u,cr:%u,type:%s", command->type.ea, command->type.cr, modem_cmux_command_type_to_str(command->type.value)); LOG_HEXDUMP_DBG(command->value, command->length.value, "data:"); } static void modem_cmux_raise_event(struct modem_cmux *cmux, enum modem_cmux_event event) { if (cmux->callback == NULL) { return; } cmux->callback(cmux, event, cmux->user_data); } static void modem_cmux_bus_callback(struct modem_pipe *pipe, enum modem_pipe_event event, void *user_data) { struct modem_cmux *cmux = (struct modem_cmux *)user_data; switch (event) { case MODEM_PIPE_EVENT_RECEIVE_READY: k_work_schedule(&cmux->receive_work, K_NO_WAIT); break; case MODEM_PIPE_EVENT_TRANSMIT_IDLE: k_work_schedule(&cmux->transmit_work, K_NO_WAIT); break; default: break; } } static uint16_t modem_cmux_transmit_frame(struct modem_cmux *cmux, const struct modem_cmux_frame *frame) { uint8_t buf[MODEM_CMUX_FRAME_SIZE_MAX]; uint8_t fcs; uint16_t space; uint16_t data_len; uint16_t buf_idx; space = ring_buf_space_get(&cmux->transmit_rb) - MODEM_CMUX_FRAME_SIZE_MAX; data_len = MIN(space, frame->data_len); /* SOF */ buf[0] = 0xF9; /* DLCI Address (Max 63) */ buf[1] = 0x01 | (frame->cr << 1) | (frame->dlci_address << 2); /* Frame type and poll/final */ buf[2] = frame->type | (frame->pf << 4); /* Data length */ if (data_len > 127) { buf[3] = data_len << 1; buf[4] = data_len >> 7; buf_idx = 5; } else { buf[3] = 0x01 | (data_len << 1); buf_idx = 4; } /* Compute FCS for the header (exclude SOF) */ fcs = crc8_rohc(MODEM_CMUX_FCS_INIT_VALUE, &buf[1], (buf_idx - 1)); /* FCS final */ if (frame->type == MODEM_CMUX_FRAME_TYPE_UIH) { fcs = 0xFF - fcs; } else { fcs = 0xFF - crc8_rohc(fcs, frame->data, data_len); } /* Frame header */ ring_buf_put(&cmux->transmit_rb, buf, buf_idx); /* Data */ ring_buf_put(&cmux->transmit_rb, frame->data, data_len); /* FCS and EOF will be put on the same call */ buf[0] = fcs; buf[1] = 0xF9; ring_buf_put(&cmux->transmit_rb, buf, 2); k_work_schedule(&cmux->transmit_work, K_NO_WAIT); return data_len; } static bool modem_cmux_transmit_cmd_frame(struct modem_cmux *cmux, const struct modem_cmux_frame *frame) { uint16_t space; struct modem_cmux_command *command; k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER); space = ring_buf_space_get(&cmux->transmit_rb); if (space < MODEM_CMUX_CMD_FRAME_SIZE_MAX) { k_mutex_unlock(&cmux->transmit_rb_lock); return false; } modem_cmux_log_transmit_frame(frame); if (modem_cmux_wrap_command(&command, frame->data, frame->data_len) == 0) { modem_cmux_log_transmit_command(command); } modem_cmux_transmit_frame(cmux, frame); k_mutex_unlock(&cmux->transmit_rb_lock); return true; } static int16_t modem_cmux_transmit_data_frame(struct modem_cmux *cmux, const struct modem_cmux_frame *frame) { uint16_t space; int ret; k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER); if (cmux->flow_control_on == false) { k_mutex_unlock(&cmux->transmit_rb_lock); return 0; } space = ring_buf_space_get(&cmux->transmit_rb); /* * Two command frames are reserved for command channel, and we shall prefer * waiting for more than MODEM_CMUX_DATA_FRAME_SIZE_MIN bytes available in the * transmit buffer rather than transmitting a few bytes at a time. This avoids * excessive wrapping overhead, since transmitting a single byte will require 8 * bytes of wrapping. */ if (space < ((MODEM_CMUX_CMD_FRAME_SIZE_MAX * 2) + MODEM_CMUX_DATA_FRAME_SIZE_MIN)) { k_mutex_unlock(&cmux->transmit_rb_lock); return -ENOMEM; } modem_cmux_log_transmit_frame(frame); ret = modem_cmux_transmit_frame(cmux, frame); k_mutex_unlock(&cmux->transmit_rb_lock); return ret; } static void modem_cmux_acknowledge_received_frame(struct modem_cmux *cmux) { struct modem_cmux_command *command; struct modem_cmux_frame frame; uint8_t data[MODEM_CMUX_CMD_DATA_SIZE_MAX]; if (sizeof(data) < cmux->frame.data_len) { LOG_WRN("Command acknowledge buffer overrun"); return; } memcpy(&frame, &cmux->frame, sizeof(cmux->frame)); memcpy(data, cmux->frame.data, cmux->frame.data_len); modem_cmux_wrap_command(&command, data, cmux->frame.data_len); command->type.cr = 0; frame.data = data; frame.data_len = cmux->frame.data_len; if (modem_cmux_transmit_cmd_frame(cmux, &frame) == false) { LOG_WRN("Command acknowledge buffer overrun"); } } 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); } } static void modem_cmux_on_fcon_command(struct modem_cmux *cmux) { k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER); cmux->flow_control_on = true; k_mutex_unlock(&cmux->transmit_rb_lock); modem_cmux_acknowledge_received_frame(cmux); } static void modem_cmux_on_fcoff_command(struct modem_cmux *cmux) { k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER); cmux->flow_control_on = false; k_mutex_unlock(&cmux->transmit_rb_lock); modem_cmux_acknowledge_received_frame(cmux); } static void modem_cmux_on_cld_command(struct modem_cmux *cmux, struct modem_cmux_command *command) { if (command->type.cr) { modem_cmux_acknowledge_received_frame(cmux); } if (cmux->state != MODEM_CMUX_STATE_DISCONNECTING && cmux->state != MODEM_CMUX_STATE_CONNECTED) { LOG_WRN("Unexpected close down"); return; } if (cmux->state == MODEM_CMUX_STATE_DISCONNECTING) { k_work_cancel_delayable(&cmux->disconnect_work); } cmux->state = MODEM_CMUX_STATE_DISCONNECTED; k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER); cmux->flow_control_on = false; k_mutex_unlock(&cmux->transmit_rb_lock); modem_cmux_raise_event(cmux, MODEM_CMUX_EVENT_DISCONNECTED); k_event_clear(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT); k_event_post(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT); } static void modem_cmux_on_control_frame_ua(struct modem_cmux *cmux) { if (cmux->state != MODEM_CMUX_STATE_CONNECTING) { LOG_DBG("Unexpected UA frame"); return; } cmux->state = MODEM_CMUX_STATE_CONNECTED; k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER); cmux->flow_control_on = true; k_mutex_unlock(&cmux->transmit_rb_lock); k_work_cancel_delayable(&cmux->connect_work); modem_cmux_raise_event(cmux, MODEM_CMUX_EVENT_CONNECTED); k_event_clear(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT); k_event_post(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT); } 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)) { LOG_DBG("Unexpected UIH frame"); return; } if (modem_cmux_wrap_command(&command, cmux->frame.data, cmux->frame.data_len) < 0) { LOG_WRN("Invalid command"); return; } modem_cmux_log_received_command(command); switch (command->type.value) { case MODEM_CMUX_COMMAND_CLD: modem_cmux_on_cld_command(cmux, command); break; case MODEM_CMUX_COMMAND_MSC: modem_cmux_on_msc_command(cmux, command); break; case MODEM_CMUX_COMMAND_FCON: modem_cmux_on_fcon_command(cmux); break; case MODEM_CMUX_COMMAND_FCOFF: modem_cmux_on_fcoff_command(cmux); break; default: LOG_DBG("Unknown control command"); break; } } static void modem_cmux_connect_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 = cmux->frame.pf, .type = MODEM_CMUX_FRAME_TYPE_UA, .data = NULL, .data_len = 0, }; LOG_DBG("SABM/DISC request state send ack"); 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); if ((cmux->state == MODEM_CMUX_STATE_CONNECTED) || (cmux->state == MODEM_CMUX_STATE_DISCONNECTING)) { LOG_DBG("Connect request not accepted"); return; } cmux->state = MODEM_CMUX_STATE_CONNECTED; k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER); cmux->flow_control_on = true; k_mutex_unlock(&cmux->transmit_rb_lock); modem_cmux_raise_event(cmux, MODEM_CMUX_EVENT_CONNECTED); k_event_clear(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT); k_event_post(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT); } static void modem_cmux_on_control_frame(struct modem_cmux *cmux) { modem_cmux_log_received_frame(&cmux->frame); switch (cmux->frame.type) { case MODEM_CMUX_FRAME_TYPE_UA: modem_cmux_on_control_frame_ua(cmux); break; case MODEM_CMUX_FRAME_TYPE_UIH: modem_cmux_on_control_frame_uih(cmux); break; case MODEM_CMUX_FRAME_TYPE_SABM: modem_cmux_on_control_frame_sabm(cmux); break; default: LOG_WRN("Unknown %s frame type", "control"); break; } } static struct modem_cmux_dlci *modem_cmux_find_dlci(struct modem_cmux *cmux) { 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) { return dlci; } } return NULL; } static void modem_cmux_on_dlci_frame_ua(struct modem_cmux_dlci *dlci) { switch (dlci->state) { case MODEM_CMUX_DLCI_STATE_OPENING: dlci->state = MODEM_CMUX_DLCI_STATE_OPEN; modem_pipe_notify_opened(&dlci->pipe); k_work_cancel_delayable(&dlci->open_work); k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER); ring_buf_reset(&dlci->receive_rb); k_mutex_unlock(&dlci->receive_rb_lock); 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); break; default: LOG_DBG("Unexpected UA frame"); break; } } static void modem_cmux_on_dlci_frame_uih(struct modem_cmux_dlci *dlci) { struct modem_cmux *cmux = dlci->cmux; uint32_t written; if (dlci->state != MODEM_CMUX_DLCI_STATE_OPEN) { LOG_DBG("Unexpected UIH frame"); return; } k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER); written = ring_buf_put(&dlci->receive_rb, cmux->frame.data, cmux->frame.data_len); k_mutex_unlock(&dlci->receive_rb_lock); if (written != cmux->frame.data_len) { 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); } modem_pipe_notify_receive_ready(&dlci->pipe); } static void modem_cmux_on_dlci_frame_sabm(struct modem_cmux_dlci *dlci) { struct modem_cmux *cmux = dlci->cmux; modem_cmux_connect_response_transmit(cmux); if (dlci->state == MODEM_CMUX_DLCI_STATE_OPEN) { LOG_DBG("Unexpected SABM frame"); return; } dlci->state = MODEM_CMUX_DLCI_STATE_OPEN; modem_pipe_notify_opened(&dlci->pipe); k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER); ring_buf_reset(&dlci->receive_rb); k_mutex_unlock(&dlci->receive_rb_lock); } static void modem_cmux_on_dlci_frame_disc(struct modem_cmux_dlci *dlci) { struct modem_cmux *cmux = dlci->cmux; modem_cmux_connect_response_transmit(cmux); if (dlci->state != MODEM_CMUX_DLCI_STATE_OPEN) { LOG_DBG("Unexpected Disc frame"); return; } dlci->state = MODEM_CMUX_DLCI_STATE_CLOSED; modem_pipe_notify_closed(&dlci->pipe); } static void modem_cmux_on_dlci_frame(struct modem_cmux *cmux) { struct modem_cmux_dlci *dlci; modem_cmux_log_received_frame(&cmux->frame); dlci = modem_cmux_find_dlci(cmux); if (dlci == NULL) { LOG_WRN("Ignoring frame intended for unconfigured DLCI %u.", cmux->frame.dlci_address); return; } switch (cmux->frame.type) { case MODEM_CMUX_FRAME_TYPE_UA: modem_cmux_on_dlci_frame_ua(dlci); break; case MODEM_CMUX_FRAME_TYPE_UIH: modem_cmux_on_dlci_frame_uih(dlci); break; case MODEM_CMUX_FRAME_TYPE_SABM: modem_cmux_on_dlci_frame_sabm(dlci); break; case MODEM_CMUX_FRAME_TYPE_DISC: modem_cmux_on_dlci_frame_disc(dlci); break; default: LOG_WRN("Unknown %s frame type", "DLCI"); break; } } static void modem_cmux_on_frame(struct modem_cmux *cmux) { #if CONFIG_MODEM_STATS modem_cmux_advertise_receive_buf_stats(cmux); #endif if (cmux->frame.dlci_address == 0) { modem_cmux_on_control_frame(cmux); } else { modem_cmux_on_dlci_frame(cmux); } } static void modem_cmux_drop_frame(struct modem_cmux *cmux) { #if CONFIG_MODEM_STATS modem_cmux_advertise_receive_buf_stats(cmux); #endif LOG_WRN("Dropped frame"); cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_SOF; #if defined(CONFIG_MODEM_CMUX_LOG_LEVEL_DBG) struct modem_cmux_frame *frame = &cmux->frame; frame->data = cmux->receive_buf; modem_cmux_log_frame(frame, "dropped", MIN(frame->data_len, cmux->receive_buf_size)); #endif } static void modem_cmux_process_received_byte(struct modem_cmux *cmux, uint8_t byte) { uint8_t fcs; switch (cmux->receive_state) { case MODEM_CMUX_RECEIVE_STATE_SOF: if (byte == 0xF9) { cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_RESYNC; break; } break; case MODEM_CMUX_RECEIVE_STATE_RESYNC: /* * Allow any number of consecutive flags (0xF9). * 0xF9 could also be a valid address field for DLCI 62. */ if (byte == 0xF9) { break; } __fallthrough; case MODEM_CMUX_RECEIVE_STATE_ADDRESS: /* Initialize */ cmux->receive_buf_len = 0; cmux->frame_header_len = 0; /* Store header for FCS */ cmux->frame_header[cmux->frame_header_len] = byte; cmux->frame_header_len++; /* Get CR */ cmux->frame.cr = (byte & 0x02) ? true : false; /* Get DLCI address */ cmux->frame.dlci_address = (byte >> 2) & 0x3F; /* Await control */ cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_CONTROL; break; case MODEM_CMUX_RECEIVE_STATE_CONTROL: /* Store header for FCS */ cmux->frame_header[cmux->frame_header_len] = byte; cmux->frame_header_len++; /* Get PF */ cmux->frame.pf = (byte & MODEM_CMUX_PF) ? true : false; /* Get frame type */ cmux->frame.type = byte & (~MODEM_CMUX_PF); /* Await data length */ cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_LENGTH; break; case MODEM_CMUX_RECEIVE_STATE_LENGTH: /* Store header for FCS */ cmux->frame_header[cmux->frame_header_len] = byte; cmux->frame_header_len++; /* Get first 7 bits of data length */ cmux->frame.data_len = (byte >> 1); /* Check if length field continues */ if ((byte & MODEM_CMUX_EA) == 0) { /* Await continued length field */ cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_LENGTH_CONT; break; } /* Check if no data field */ if (cmux->frame.data_len == 0) { /* Await FCS */ cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_FCS; break; } /* Await data */ cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DATA; break; case MODEM_CMUX_RECEIVE_STATE_LENGTH_CONT: /* Store header for FCS */ cmux->frame_header[cmux->frame_header_len] = byte; cmux->frame_header_len++; /* Get last 8 bits of data length */ cmux->frame.data_len |= ((uint16_t)byte) << 7; if (cmux->frame.data_len > cmux->receive_buf_size) { LOG_ERR("Indicated frame data length %u exceeds receive buffer size %u", cmux->frame.data_len, cmux->receive_buf_size); cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DROP; break; } /* Await data */ cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DATA; break; case MODEM_CMUX_RECEIVE_STATE_DATA: /* Copy byte to data */ if (cmux->receive_buf_len < cmux->receive_buf_size) { cmux->receive_buf[cmux->receive_buf_len] = byte; } cmux->receive_buf_len++; /* Check if datalen reached */ if (cmux->frame.data_len == cmux->receive_buf_len) { /* Await FCS */ cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_FCS; } break; case MODEM_CMUX_RECEIVE_STATE_FCS: if (cmux->receive_buf_len > cmux->receive_buf_size) { LOG_WRN("Receive buffer overrun (%u > %u)", cmux->receive_buf_len, cmux->receive_buf_size); cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DROP; break; } /* Compute FCS */ fcs = crc8_rohc(MODEM_CMUX_FCS_INIT_VALUE, cmux->frame_header, cmux->frame_header_len); if (cmux->frame.type == MODEM_CMUX_FRAME_TYPE_UIH) { fcs = 0xFF - fcs; } else { fcs = 0xFF - crc8_rohc(fcs, cmux->frame.data, cmux->frame.data_len); } /* Validate FCS */ if (fcs != byte) { LOG_WRN("Frame FCS error"); /* Drop frame */ cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DROP; break; } cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_EOF; break; case MODEM_CMUX_RECEIVE_STATE_DROP: modem_cmux_drop_frame(cmux); break; case MODEM_CMUX_RECEIVE_STATE_EOF: /* Validate byte is EOF */ if (byte != 0xF9) { /* Unexpected byte */ modem_cmux_drop_frame(cmux); break; } /* Process frame */ cmux->frame.data = cmux->receive_buf; modem_cmux_on_frame(cmux); /* Await start of next frame */ cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_SOF; break; default: break; } } static void modem_cmux_receive_handler(struct k_work *item) { struct k_work_delayable *dwork = k_work_delayable_from_work(item); struct modem_cmux *cmux = CONTAINER_OF(dwork, struct modem_cmux, receive_work); 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); } return; } /* Process received data */ for (int i = 0; i < ret; i++) { modem_cmux_process_received_byte(cmux, cmux->work_buf[i]); } /* Reschedule received work */ k_work_schedule(&cmux->receive_work, K_NO_WAIT); } static void modem_cmux_dlci_notify_transmit_idle(struct modem_cmux *cmux) { sys_snode_t *node; struct modem_cmux_dlci *dlci; SYS_SLIST_FOR_EACH_NODE(&cmux->dlcis, node) { dlci = (struct modem_cmux_dlci *)node; modem_pipe_notify_transmit_idle(&dlci->pipe); } } static void modem_cmux_transmit_handler(struct k_work *item) { struct k_work_delayable *dwork = k_work_delayable_from_work(item); struct modem_cmux *cmux = CONTAINER_OF(dwork, struct modem_cmux, transmit_work); uint8_t *reserved; uint32_t reserved_size; int ret; bool transmit_rb_empty; k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER); #if CONFIG_MODEM_STATS modem_cmux_advertise_transmit_buf_stats(cmux); #endif while (true) { transmit_rb_empty = ring_buf_is_empty(&cmux->transmit_rb); if (transmit_rb_empty) { break; } reserved_size = ring_buf_get_claim(&cmux->transmit_rb, &reserved, UINT32_MAX); ret = modem_pipe_transmit(cmux->pipe, reserved, reserved_size); if (ret < 0) { ring_buf_get_finish(&cmux->transmit_rb, 0); if (ret != -EPERM) { LOG_ERR("Failed to %s %u bytes. (%d)", "transmit", reserved_size, ret); } break; } ring_buf_get_finish(&cmux->transmit_rb, (uint32_t)ret); if (ret < reserved_size) { LOG_DBG("Transmitted only %u out of %u bytes at once.", ret, reserved_size); break; } } k_mutex_unlock(&cmux->transmit_rb_lock); if (transmit_rb_empty) { modem_cmux_dlci_notify_transmit_idle(cmux); } } static void modem_cmux_connect_handler(struct k_work *item) { struct k_work_delayable *dwork; struct modem_cmux *cmux; if (item == NULL) { return; } dwork = k_work_delayable_from_work(item); cmux = CONTAINER_OF(dwork, struct modem_cmux, connect_work); cmux->state = MODEM_CMUX_STATE_CONNECTING; static const struct modem_cmux_frame frame = { .dlci_address = 0, .cr = true, .pf = true, .type = MODEM_CMUX_FRAME_TYPE_SABM, .data = NULL, .data_len = 0, }; modem_cmux_transmit_cmd_frame(cmux, &frame); k_work_schedule(&cmux->connect_work, MODEM_CMUX_T1_TIMEOUT); } static void modem_cmux_disconnect_handler(struct k_work *item) { struct k_work_delayable *dwork = k_work_delayable_from_work(item); struct modem_cmux *cmux = CONTAINER_OF(dwork, struct modem_cmux, disconnect_work); struct modem_cmux_command *command; uint8_t data[2]; cmux->state = MODEM_CMUX_STATE_DISCONNECTING; command = modem_cmux_command_wrap(data); command->type.ea = 1; command->type.cr = 1; command->type.value = MODEM_CMUX_COMMAND_CLD; command->length.ea = 1; command->length.value = 0; struct modem_cmux_frame frame = { .dlci_address = 0, .cr = true, .pf = false, .type = MODEM_CMUX_FRAME_TYPE_UIH, .data = data, .data_len = sizeof(data), }; /* Transmit close down command */ modem_cmux_transmit_cmd_frame(cmux, &frame); k_work_schedule(&cmux->disconnect_work, MODEM_CMUX_T1_TIMEOUT); } #if CONFIG_MODEM_STATS static uint32_t modem_cmux_dlci_get_receive_buf_length(struct modem_cmux_dlci *dlci) { return ring_buf_size_get(&dlci->receive_rb); } static uint32_t modem_cmux_dlci_get_receive_buf_size(struct modem_cmux_dlci *dlci) { return ring_buf_capacity_get(&dlci->receive_rb); } static void modem_cmux_dlci_init_buf_stats(struct modem_cmux_dlci *dlci) { uint32_t size; char name[sizeof("dlci_xxxxx_rx")]; size = modem_cmux_dlci_get_receive_buf_size(dlci); snprintk(name, sizeof(name), "dlci_%u_rx", dlci->dlci_address); modem_stats_buffer_init(&dlci->receive_buf_stats, name, size); } static void modem_cmux_dlci_advertise_receive_buf_stat(struct modem_cmux_dlci *dlci) { uint32_t length; length = modem_cmux_dlci_get_receive_buf_length(dlci); modem_stats_buffer_advertise_length(&dlci->receive_buf_stats, length); } #endif static int modem_cmux_dlci_pipe_api_open(void *data) { struct modem_cmux_dlci *dlci = (struct modem_cmux_dlci *)data; struct modem_cmux *cmux = dlci->cmux; int ret = 0; K_SPINLOCK(&cmux->work_lock) { if (!cmux->attached) { ret = -EPERM; K_SPINLOCK_BREAK; } if (k_work_delayable_is_pending(&dlci->open_work) == true) { ret = -EBUSY; K_SPINLOCK_BREAK; } k_work_schedule(&dlci->open_work, K_NO_WAIT); } return ret; } static int modem_cmux_dlci_pipe_api_transmit(void *data, const uint8_t *buf, size_t size) { struct modem_cmux_dlci *dlci = (struct modem_cmux_dlci *)data; struct modem_cmux *cmux = dlci->cmux; int ret; K_SPINLOCK(&cmux->work_lock) { if (!cmux->attached) { ret = -EPERM; K_SPINLOCK_BREAK; } struct modem_cmux_frame frame = { .dlci_address = dlci->dlci_address, .cr = true, .pf = false, .type = MODEM_CMUX_FRAME_TYPE_UIH, .data = buf, .data_len = size, }; ret = modem_cmux_transmit_data_frame(cmux, &frame); } return ret; } static int modem_cmux_dlci_pipe_api_receive(void *data, uint8_t *buf, size_t size) { struct modem_cmux_dlci *dlci = (struct modem_cmux_dlci *)data; uint32_t ret; k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER); #if CONFIG_MODEM_STATS modem_cmux_dlci_advertise_receive_buf_stat(dlci); #endif ret = ring_buf_get(&dlci->receive_rb, buf, size); k_mutex_unlock(&dlci->receive_rb_lock); return ret; } static int modem_cmux_dlci_pipe_api_close(void *data) { struct modem_cmux_dlci *dlci = (struct modem_cmux_dlci *)data; struct modem_cmux *cmux = dlci->cmux; int ret = 0; K_SPINLOCK(&cmux->work_lock) { if (!cmux->attached) { ret = -EPERM; K_SPINLOCK_BREAK; } if (k_work_delayable_is_pending(&dlci->close_work) == true) { ret = -EBUSY; K_SPINLOCK_BREAK; } k_work_schedule(&dlci->close_work, K_NO_WAIT); } return ret; } static const struct modem_pipe_api modem_cmux_dlci_pipe_api = { .open = modem_cmux_dlci_pipe_api_open, .transmit = modem_cmux_dlci_pipe_api_transmit, .receive = modem_cmux_dlci_pipe_api_receive, .close = modem_cmux_dlci_pipe_api_close, }; static void modem_cmux_dlci_open_handler(struct k_work *item) { struct k_work_delayable *dwork; struct modem_cmux_dlci *dlci; if (item == NULL) { return; } dwork = k_work_delayable_from_work(item); dlci = CONTAINER_OF(dwork, struct modem_cmux_dlci, open_work); dlci->state = MODEM_CMUX_DLCI_STATE_OPENING; struct modem_cmux_frame frame = { .dlci_address = dlci->dlci_address, .cr = true, .pf = true, .type = MODEM_CMUX_FRAME_TYPE_SABM, .data = NULL, .data_len = 0, }; modem_cmux_transmit_cmd_frame(dlci->cmux, &frame); k_work_schedule(&dlci->open_work, MODEM_CMUX_T1_TIMEOUT); } static void modem_cmux_dlci_close_handler(struct k_work *item) { struct k_work_delayable *dwork; struct modem_cmux_dlci *dlci; struct modem_cmux *cmux; if (item == NULL) { return; } dwork = k_work_delayable_from_work(item); dlci = CONTAINER_OF(dwork, struct modem_cmux_dlci, close_work); cmux = dlci->cmux; dlci->state = MODEM_CMUX_DLCI_STATE_CLOSING; struct modem_cmux_frame frame = { .dlci_address = dlci->dlci_address, .cr = true, .pf = true, .type = MODEM_CMUX_FRAME_TYPE_DISC, .data = NULL, .data_len = 0, }; modem_cmux_transmit_cmd_frame(cmux, &frame); k_work_schedule(&dlci->close_work, MODEM_CMUX_T1_TIMEOUT); } static void modem_cmux_dlci_pipes_release(struct modem_cmux *cmux) { sys_snode_t *node; struct modem_cmux_dlci *dlci; struct k_work_sync sync; SYS_SLIST_FOR_EACH_NODE(&cmux->dlcis, node) { dlci = (struct modem_cmux_dlci *)node; modem_pipe_notify_closed(&dlci->pipe); k_work_cancel_delayable_sync(&dlci->open_work, &sync); k_work_cancel_delayable_sync(&dlci->close_work, &sync); } } void modem_cmux_init(struct modem_cmux *cmux, const struct modem_cmux_config *config) { __ASSERT_NO_MSG(cmux != NULL); __ASSERT_NO_MSG(config != NULL); __ASSERT_NO_MSG(config->receive_buf != NULL); __ASSERT_NO_MSG(config->receive_buf_size >= 126); __ASSERT_NO_MSG(config->transmit_buf != NULL); __ASSERT_NO_MSG(config->transmit_buf_size >= 148); memset(cmux, 0x00, sizeof(*cmux)); cmux->callback = config->callback; cmux->user_data = config->user_data; cmux->receive_buf = config->receive_buf; cmux->receive_buf_size = config->receive_buf_size; sys_slist_init(&cmux->dlcis); cmux->state = MODEM_CMUX_STATE_DISCONNECTED; ring_buf_init(&cmux->transmit_rb, config->transmit_buf_size, config->transmit_buf); k_mutex_init(&cmux->transmit_rb_lock); k_work_init_delayable(&cmux->receive_work, modem_cmux_receive_handler); k_work_init_delayable(&cmux->transmit_work, modem_cmux_transmit_handler); k_work_init_delayable(&cmux->connect_work, modem_cmux_connect_handler); k_work_init_delayable(&cmux->disconnect_work, modem_cmux_disconnect_handler); k_event_init(&cmux->event); k_event_clear(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT); k_event_post(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT); #if CONFIG_MODEM_STATS modem_cmux_init_buf_stats(cmux); #endif } struct modem_pipe *modem_cmux_dlci_init(struct modem_cmux *cmux, struct modem_cmux_dlci *dlci, const struct modem_cmux_dlci_config *config) { __ASSERT_NO_MSG(cmux != NULL); __ASSERT_NO_MSG(dlci != NULL); __ASSERT_NO_MSG(config != NULL); __ASSERT_NO_MSG(config->dlci_address < 64); __ASSERT_NO_MSG(config->receive_buf != NULL); __ASSERT_NO_MSG(config->receive_buf_size >= 126); memset(dlci, 0x00, sizeof(*dlci)); dlci->cmux = cmux; dlci->dlci_address = config->dlci_address; ring_buf_init(&dlci->receive_rb, config->receive_buf_size, config->receive_buf); k_mutex_init(&dlci->receive_rb_lock); modem_pipe_init(&dlci->pipe, dlci, &modem_cmux_dlci_pipe_api); k_work_init_delayable(&dlci->open_work, modem_cmux_dlci_open_handler); k_work_init_delayable(&dlci->close_work, modem_cmux_dlci_close_handler); dlci->state = MODEM_CMUX_DLCI_STATE_CLOSED; sys_slist_append(&dlci->cmux->dlcis, &dlci->node); #if CONFIG_MODEM_STATS modem_cmux_dlci_init_buf_stats(dlci); #endif return &dlci->pipe; } int modem_cmux_attach(struct modem_cmux *cmux, struct modem_pipe *pipe) { if (cmux->pipe != NULL) { return -EALREADY; } cmux->pipe = pipe; ring_buf_reset(&cmux->transmit_rb); cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_SOF; modem_pipe_attach(cmux->pipe, modem_cmux_bus_callback, cmux); K_SPINLOCK(&cmux->work_lock) { cmux->attached = true; } return 0; } int modem_cmux_connect(struct modem_cmux *cmux) { int ret; ret = modem_cmux_connect_async(cmux); if (ret < 0) { return ret; } if (k_event_wait(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT, false, MODEM_CMUX_T2_TIMEOUT) == 0) { return -EAGAIN; } return 0; } int modem_cmux_connect_async(struct modem_cmux *cmux) { int ret; if (k_event_test(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT)) { return -EALREADY; } K_SPINLOCK(&cmux->work_lock) { if (!cmux->attached) { ret = -EPERM; K_SPINLOCK_BREAK; } if (k_work_delayable_is_pending(&cmux->connect_work) == false) { k_work_schedule(&cmux->connect_work, K_NO_WAIT); } ret = 0; } return ret; } int modem_cmux_disconnect(struct modem_cmux *cmux) { int ret; ret = modem_cmux_disconnect_async(cmux); if (ret < 0) { return ret; } if (k_event_wait(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT, false, MODEM_CMUX_T2_TIMEOUT) == 0) { return -EAGAIN; } return 0; } int modem_cmux_disconnect_async(struct modem_cmux *cmux) { int ret = 0; if (k_event_test(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT)) { return -EALREADY; } K_SPINLOCK(&cmux->work_lock) { if (!cmux->attached) { ret = -EPERM; K_SPINLOCK_BREAK; } if (k_work_delayable_is_pending(&cmux->disconnect_work) == false) { k_work_schedule(&cmux->disconnect_work, K_NO_WAIT); } } return ret; } void modem_cmux_release(struct modem_cmux *cmux) { struct k_work_sync sync; if (cmux->pipe == NULL) { return; } K_SPINLOCK(&cmux->work_lock) { cmux->attached = false; } /* Close DLCI pipes and cancel DLCI work */ modem_cmux_dlci_pipes_release(cmux); /* Release bus pipe */ if (cmux->pipe) { modem_pipe_release(cmux->pipe); } /* Cancel all work */ k_work_cancel_delayable_sync(&cmux->connect_work, &sync); k_work_cancel_delayable_sync(&cmux->disconnect_work, &sync); k_work_cancel_delayable_sync(&cmux->transmit_work, &sync); k_work_cancel_delayable_sync(&cmux->receive_work, &sync); /* Unreference pipe */ cmux->pipe = NULL; /* Reset events */ k_event_clear(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT); k_event_post(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT); }