zephyr/subsys/modem/modem_cmux.c

/*
 * Copyright (c) 2022 Trackunit Corporation
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(modem_cmux, CONFIG_MODEM_MODULES_LOG_LEVEL);

#include <zephyr/kernel.h>
#include <zephyr/sys/crc.h>
#include <zephyr/modem/cmux.h>

#include <string.h>

#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(uint8_t *data)
{
	return (struct modem_cmux_command *)data;
}

static void modem_cmux_log_unknown_frame(struct modem_cmux *cmux)
{
	char data[24];
	uint8_t data_cnt = (cmux->frame.data_len < 8) ? cmux->frame.data_len : 8;

	for (uint8_t i = 0; i < data_cnt; i++) {
		snprintk(&data[i * 3], sizeof(data) - (i * 3), "%02X,", cmux->frame.data[i]);
	}

	/* Remove trailing */
	if (data_cnt > 0) {
		data[(data_cnt * 3) - 1] = '\0';
	}

	LOG_DBG("ch:%u, type:%u, data:%s", cmux->frame.dlci_address, cmux->frame.type, 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;

	if (event == MODEM_PIPE_EVENT_RECEIVE_READY) {
		k_work_schedule(&cmux->receive_work, K_NO_WAIT);
	}
}

static uint16_t modem_cmux_transmit_frame(struct modem_cmux *cmux,
					  const struct modem_cmux_frame *frame)
{
	uint8_t byte;
	uint8_t fcs;
	uint16_t space;
	uint16_t data_len;

	space = ring_buf_space_get(&cmux->transmit_rb) - MODEM_CMUX_FRAME_SIZE_MAX;
	data_len = (space < frame->data_len) ? space : frame->data_len;

	/* SOF */
	byte = 0xF9;
	ring_buf_put(&cmux->transmit_rb, &byte, 1);

	/* DLCI Address (Max 63) */
	byte = 0x01 | (frame->cr << 1) | (frame->dlci_address << 2);
	fcs = crc8(&byte, 1, MODEM_CMUX_FCS_POLYNOMIAL, MODEM_CMUX_FCS_INIT_VALUE, true);
	ring_buf_put(&cmux->transmit_rb, &byte, 1);

	/* Frame type and poll/final */
	byte = frame->type | (frame->pf << 4);
	fcs = crc8(&byte, 1, MODEM_CMUX_FCS_POLYNOMIAL, fcs, true);
	ring_buf_put(&cmux->transmit_rb, &byte, 1);

	/* Data length */
	if (data_len > 127) {
		byte = data_len << 1;
		fcs = crc8(&byte, 1, MODEM_CMUX_FCS_POLYNOMIAL, fcs, true);
		ring_buf_put(&cmux->transmit_rb, &byte, 1);
		byte = 0x01 | (data_len >> 7);
		ring_buf_put(&cmux->transmit_rb, &byte, 1);
	} else {
		byte = 0x01 | (data_len << 1);
		ring_buf_put(&cmux->transmit_rb, &byte, 1);
	}

	/* FCS final */
	if (frame->type == MODEM_CMUX_FRAME_TYPE_UIH) {
		fcs = 0xFF - crc8(&byte, 1, MODEM_CMUX_FCS_POLYNOMIAL, fcs, true);
	} else {
		fcs = crc8(&byte, 1, MODEM_CMUX_FCS_POLYNOMIAL, fcs, true);
		fcs = 0xFF - crc8(frame->data, data_len, MODEM_CMUX_FCS_POLYNOMIAL, fcs, true);
	}

	/* Data */
	ring_buf_put(&cmux->transmit_rb, frame->data, data_len);

	/* FCS */
	ring_buf_put(&cmux->transmit_rb, &fcs, 1);

	/* EOF */
	byte = 0xF9;
	ring_buf_put(&cmux->transmit_rb, &byte, 1);
	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;

	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_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;
	}

	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)
{
	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)
{
	if (cmux->state != MODEM_CMUX_STATE_DISCONNECTING) {
		LOG_WRN("Unexpected close down");
	}

	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);
	k_work_cancel_delayable(&cmux->disconnect_work);
	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;
	}

	switch (command->type.value) {
	case MODEM_CMUX_COMMAND_CLD:
		modem_cmux_on_cld_command(cmux);
		break;

	case MODEM_CMUX_COMMAND_MSC:
		modem_cmux_on_msc_command(cmux);
		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 command");
		break;
	}
}

static void modem_cmux_on_control_frame(struct modem_cmux *cmux)
{
	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;

	default:
		modem_cmux_log_unknown_frame(cmux);
		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);
		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;

	if (dlci->state != MODEM_CMUX_DLCI_STATE_OPEN) {
		LOG_DBG("Unexpected UIH frame");
		return;
	}

	k_mutex_lock(&dlci->receive_rb_lock, K_FOREVER);
	ring_buf_put(&dlci->receive_rb, cmux->frame.data, cmux->frame.data_len);
	k_mutex_unlock(&dlci->receive_rb_lock);
	modem_pipe_notify_receive_ready(&dlci->pipe);
}

static void modem_cmux_on_dlci_frame(struct modem_cmux *cmux)
{
	struct modem_cmux_dlci *dlci;

	dlci = modem_cmux_find_dlci(cmux);

	if (dlci == NULL) {
		LOG_WRN("Could not find 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;

	default:
		modem_cmux_log_unknown_frame(cmux);
		break;
	}
}

static void modem_cmux_on_frame(struct modem_cmux *cmux)
{
	if (cmux->frame.dlci_address == 0) {
		modem_cmux_on_control_frame(cmux);
		return;
	}

	modem_cmux_on_dlci_frame(cmux);
}

static void modem_cmux_process_received_byte(struct modem_cmux *cmux, uint8_t byte)
{
	uint8_t fcs;
	static const uint8_t resync[3] = {0xF9, 0xF9, 0xF9};

	switch (cmux->receive_state) {
	case MODEM_CMUX_RECEIVE_STATE_SOF:
		if (byte == 0xF9) {
			cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_ADDRESS;
			break;
		}

		/* Send resync flags */
		modem_pipe_transmit(cmux->pipe, resync, sizeof(resync));

		/* Await resync flags */
		cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_RESYNC_0;
		break;

	case MODEM_CMUX_RECEIVE_STATE_RESYNC_0:
		if (byte == 0xF9) {
			cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_RESYNC_1;
		}

		break;

	case MODEM_CMUX_RECEIVE_STATE_RESYNC_1:
		if (byte == 0xF9) {
			cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_RESYNC_2;
		} else {
			cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_RESYNC_0;
		}

		break;

	case MODEM_CMUX_RECEIVE_STATE_RESYNC_2:
		if (byte == 0xF9) {
			cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_RESYNC_3;
		} else {
			cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_RESYNC_0;
		}

		break;

	case MODEM_CMUX_RECEIVE_STATE_RESYNC_3:
		if (byte == 0xF9) {
			break;
		}

	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;

		/* Await data */
		cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_DATA;
		break;

	case MODEM_CMUX_RECEIVE_STATE_DATA:
		/* Copy byte to data */
		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;
		}

		/* Check if receive buffer overrun */
		if (cmux->receive_buf_len == cmux->receive_buf_size) {
			LOG_DBG("Receive buf overrun");

			/* Drop frame */
			cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_EOF;
			break;
		}

		break;

	case MODEM_CMUX_RECEIVE_STATE_FCS:
		/* Compute FCS */
		if (cmux->frame.type == MODEM_CMUX_FRAME_TYPE_UIH) {
			fcs = 0xFF - crc8(cmux->frame_header, cmux->frame_header_len,
					  MODEM_CMUX_FCS_POLYNOMIAL, MODEM_CMUX_FCS_INIT_VALUE,
					  true);
		} else {
			fcs = crc8(cmux->frame_header, cmux->frame_header_len,
				   MODEM_CMUX_FCS_POLYNOMIAL, MODEM_CMUX_FCS_INIT_VALUE, true);

			fcs = 0xFF - crc8(cmux->frame.data, cmux->frame.data_len,
					  MODEM_CMUX_FCS_POLYNOMIAL, fcs, true);
		}

		/* 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:
		LOG_WRN("Dropped frame");
		cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_SOF;
		break;

	case MODEM_CMUX_RECEIVE_STATE_EOF:
		/* Validate byte is EOF */
		if (byte != 0xF9) {
			/* Unexpected byte */
			cmux->receive_state = MODEM_CMUX_RECEIVE_STATE_SOF;
			break;
		}

		LOG_DBG("Received frame");

		/* 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);
	uint8_t buf[16];
	int ret;

	/* Receive data from pipe */
	ret = modem_pipe_receive(cmux->pipe, buf, sizeof(buf));
	if (ret < 1) {
		return;
	}

	/* Process received data */
	for (uint16_t i = 0; i < (uint16_t)ret; i++) {
		modem_cmux_process_received_byte(cmux, buf[i]);
	}

	/* Reschedule received work */
	k_work_schedule(&cmux->receive_work, K_NO_WAIT);
}

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;

	k_mutex_lock(&cmux->transmit_rb_lock, K_FOREVER);

	/* Reserve data to transmit from transmit ring buffer */
	reserved_size = ring_buf_get_claim(&cmux->transmit_rb, &reserved, UINT32_MAX);

	/* Transmit reserved data */
	ret = modem_pipe_transmit(cmux->pipe, reserved, reserved_size);
	if (ret < 1) {
		ring_buf_get_finish(&cmux->transmit_rb, 0);
		k_mutex_unlock(&cmux->transmit_rb_lock);
		k_work_schedule(&cmux->transmit_work, K_NO_WAIT);

		return;
	}

	/* Release remaining reserved data */
	ring_buf_get_finish(&cmux->transmit_rb, ret);

	/* Resubmit transmit work if data remains */
	if (ring_buf_is_empty(&cmux->transmit_rb) == false) {
		k_work_schedule(&cmux->transmit_work, K_NO_WAIT);
	}

	k_mutex_unlock(&cmux->transmit_rb_lock);
}

static void modem_cmux_connect_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, connect_work);

	cmux->state = MODEM_CMUX_STATE_CONNECTING;

	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);
}

static int modem_cmux_dlci_pipe_api_open(void *data)
{
	struct modem_cmux_dlci *dlci = (struct modem_cmux_dlci *)data;

	if (k_work_delayable_is_pending(&dlci->open_work) == true) {
		return -EBUSY;
	}

	k_work_schedule(&dlci->open_work, K_NO_WAIT);
	return 0;
}

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;

	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,
	};

	return modem_cmux_transmit_data_frame(cmux, &frame);
}

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);
	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;

	if (k_work_delayable_is_pending(&dlci->close_work) == true) {
		return -EBUSY;
	}

	k_work_schedule(&dlci->close_work, K_NO_WAIT);
	return 0;
}

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 = k_work_delayable_from_work(item);
	struct modem_cmux_dlci *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 = k_work_delayable_from_work(item);
	struct modem_cmux_dlci *dlci = CONTAINER_OF(dwork, struct modem_cmux_dlci, close_work);
	struct modem_cmux *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_notify_closed(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_closed(&dlci->pipe);
	}
}

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_post(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT);
}

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);
	return &dlci->pipe;
}

int modem_cmux_attach(struct modem_cmux *cmux, struct modem_pipe *pipe)
{
	cmux->pipe = pipe;
	ring_buf_reset(&cmux->transmit_rb);
	modem_pipe_attach(cmux->pipe, modem_cmux_bus_callback, cmux);
	return 0;
}

int modem_cmux_connect(struct modem_cmux *cmux)
{
	__ASSERT_NO_MSG(cmux->pipe != NULL);

	if (k_event_wait(&cmux->event, MODEM_CMUX_EVENT_CONNECTED_BIT, false, K_NO_WAIT)) {
		return -EALREADY;
	}

	if (k_work_delayable_is_pending(&cmux->connect_work) == false) {
		k_work_schedule(&cmux->connect_work, K_NO_WAIT);
	}

	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)
{
	__ASSERT_NO_MSG(cmux->pipe != NULL);

	if (k_work_delayable_is_pending(&cmux->connect_work) == true) {
		return -EBUSY;
	}

	k_work_schedule(&cmux->connect_work, K_NO_WAIT);
	return 0;
}

int modem_cmux_disconnect(struct modem_cmux *cmux)
{
	if (k_event_wait(&cmux->event, MODEM_CMUX_EVENT_DISCONNECTED_BIT, false, K_NO_WAIT)) {
		return -EALREADY;
	}

	if (k_work_delayable_is_pending(&cmux->disconnect_work) == false) {
		k_work_schedule(&cmux->disconnect_work, K_NO_WAIT);
	}

	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)
{
	if (k_work_delayable_is_pending(&cmux->disconnect_work) == true) {
		return -EBUSY;
	}

	k_work_schedule(&cmux->disconnect_work, K_NO_WAIT);
	return 0;
}

void modem_cmux_release(struct modem_cmux *cmux)
{
	struct k_work_sync sync;

	/* Close DLCI pipes */
	modem_cmux_dlci_pipes_notify_closed(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;
}