/*
 * Copyright (c) 2020 Intel Corporation
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <logging/log.h>
LOG_MODULE_REGISTER(gsm_mux, CONFIG_GSM_MUX_LOG_LEVEL);

#include <kernel.h>
#include <sys/util.h>
#include <sys/crc.h>
#include <net/buf.h>
#include <net/ppp.h>

#include "uart_mux_internal.h"
#include "gsm_mux.h"

/* Default values are from the specification 07.10 */
#define T1_MSEC 100  /* 100 ms */
#define T2_MSEC 340  /* 333 ms */

#define N1 256 /* default I frame size, GSM 07.10 ch 6.2.2.1 */
#define N2 3   /* retry 3 times */

/* CRC8 is the reflected CRC8/ROHC algorithm */
#define FCS_POLYNOMIAL 0xe0 /* reversed crc8 */
#define FCS_INIT_VALUE 0xFF
#define FCS_GOOD_VALUE 0xCF

#define EA 0x01  /* Extension bit      */
#define CR 0x02  /* Command / Response */
#define PF 0x10  /* Poll / Final       */

/* Frame types */
#define FT_RR      0x01  /* Receive Ready                            */
#define FT_UI      0x03  /* Unnumbered Information                   */
#define FT_RNR     0x05  /* Receive Not Ready                        */
#define FT_REJ     0x09  /* Reject                                   */
#define FT_DM      0x0F  /* Disconnected Mode                        */
#define FT_SABM    0x2F  /* Set Asynchronous Balanced Mode           */
#define FT_DISC    0x43  /* Disconnect                               */
#define FT_UA      0x63  /* Unnumbered Acknowledgement               */
#define FT_UIH     0xEF  /* Unnumbered Information with Header check */

/* Control channel commands */
#define CMD_NSC    0x08  /* Non Supported Command Response           */
#define CMD_TEST   0x10  /* Test Command                             */
#define CMD_PSC    0x20  /* Power Saving Control                     */
#define CMD_RLS    0x28  /* Remote Line Status Command               */
#define CMD_FCOFF  0x30  /* Flow Control Off Command                 */
#define CMD_PN     0x40  /* DLC parameter negotiation                */
#define CMD_RPN    0x48  /* Remote Port Negotiation Command          */
#define CMD_FCON   0x50  /* Flow Control On Command                  */
#define CMD_CLD    0x60  /* Multiplexer close down                   */
#define CMD_SNC    0x68  /* Service Negotiation Command              */
#define CMD_MSC    0x70  /* Modem Status Command                     */

/* Flag sequence field between messages (start of frame) */
#define SOF_MARKER 0xF9

/* Mux parsing states */
enum gsm_mux_state {
	GSM_MUX_SOF,      /* Start of frame       */
	GSM_MUX_ADDRESS,  /* Address field        */
	GSM_MUX_CONTROL,  /* Control field        */
	GSM_MUX_LEN_0,    /* First length byte    */
	GSM_MUX_LEN_1,    /* Second length byte   */
	GSM_MUX_DATA,     /* Data                 */
	GSM_MUX_FCS,      /* Frame Check Sequence */
	GSM_MUX_EOF       /* End of frame         */
};

struct gsm_mux {
	/* UART device to use. This device is the real UART, not the
	 * muxed one.
	 */
	const struct device *uart;

	/* Buf to use when TX mux packet (hdr + data). For RX it only contains
	 * the data (not hdr).
	 */
	struct net_buf *buf;
	int mru;

	enum gsm_mux_state state;

	/* Control DLCI is not included in this list so -1 here */
	uint8_t dlci_to_create[CONFIG_GSM_MUX_DLCI_MAX - 1];

	uint16_t msg_len;     /* message length */
	uint16_t received;    /* bytes so far received */

	struct k_delayed_work t2_timer;
	sys_slist_t pending_ctrls;

	uint16_t t1_timeout_value; /* T1 default value */
	uint16_t t2_timeout_value; /* T2 default value */

	/* Information from currently read packet */
	uint8_t address;      /* dlci address (only one byte address supported) */
	uint8_t control;      /* type of the frame */
	uint8_t fcs;          /* calculated frame check sequence */
	uint8_t received_fcs; /* packet fcs */
	uint8_t retries;      /* N2 counter */

	bool in_use : 1;
	bool is_initiator : 1;   /* Did we initiate the connection attempt */
	bool refuse_service : 1; /* Do not try to talk to this modem */
};

/* DLCI states */
enum gsm_dlci_state {
	GSM_DLCI_CLOSED,
	GSM_DLCI_OPENING,
	GSM_DLCI_OPEN,
	GSM_DLCI_CLOSING
};

enum gsm_dlci_mode {
	GSM_DLCI_MODE_ABM = 0,  /* Normal Asynchronous Balanced Mode */
	GSM_DLCI_MODE_ADM = 1,  /* Asynchronous Disconnected Mode */
};

typedef int (*dlci_process_msg_t)(struct gsm_dlci *dlci, bool cmd,
				  struct net_buf *buf);
typedef void (*dlci_command_cb_t)(struct gsm_dlci *dlci, bool connected);

struct gsm_dlci {
	sys_snode_t node;
	struct k_sem disconnect_sem;
	struct gsm_mux *mux;
	dlci_process_msg_t handler;
	dlci_command_cb_t command_cb;
	gsm_mux_dlci_created_cb_t dlci_created_cb;
	void *user_data;
	const struct device *uart;
	enum gsm_dlci_state state;
	enum gsm_dlci_mode mode;
	int num;
	uint32_t req_start;
	uint8_t retries;
	bool refuse_service : 1; /* Do not try to talk to this channel */
	bool in_use : 1;
};

struct gsm_control_msg {
	sys_snode_t node;
	struct net_buf *buf;
	uint32_t req_start;
	uint8_t cmd;
	bool finished : 1;
};

/* From 07.10, Maximum Frame Size [1 - 128] in Basic mode */
#define MAX_MRU CONFIG_GSM_MUX_MRU_MAX_LEN

/* Assume that there are 3 network buffers (one for RX and one for TX, and one
 * extra when parsing data) going on at the same time.
 */
#define MIN_BUF_COUNT (CONFIG_GSM_MUX_MAX * 3)

NET_BUF_POOL_DEFINE(gsm_mux_pool, MIN_BUF_COUNT, MAX_MRU, 0, NULL);

#define BUF_ALLOC_TIMEOUT K_MSEC(50)

static struct gsm_mux muxes[CONFIG_GSM_MUX_MAX];

static struct gsm_dlci dlcis[CONFIG_GSM_MUX_DLCI_MAX];
static sys_slist_t dlci_free_entries;
static sys_slist_t dlci_active_t1_timers;
static struct k_delayed_work t1_timer;

static struct gsm_control_msg ctrls[CONFIG_GSM_MUX_PENDING_CMD_MAX];
static sys_slist_t ctrls_free_entries;

static bool gsm_mux_init_done;

static const char *get_frame_type_str(uint8_t frame_type)
{
	switch (frame_type) {
	case FT_RR:
		return "RR";
	case FT_UI:
		return "UI";
	case FT_RNR:
		return "RNR";
	case FT_REJ:
		return "REJ";
	case FT_DM:
		return "DM";
	case FT_SABM:
		return "SABM";
	case FT_DISC:
		return "DISC";
	case FT_UA:
		return "UA";
	case FT_UIH:
		return "UIH";
	}

	return NULL;
}

static void hexdump_packet(const char *header, uint8_t address, bool cmd_rsp,
			   uint8_t control, const uint8_t *data, size_t len)
{
	const char *frame_type;
	char out[128];
	int ret;

	if (!IS_ENABLED(CONFIG_GSM_MUX_LOG_LEVEL_DBG)) {
		return;
	}

	memset(out, 0, sizeof(out));

	ret = snprintk(out, sizeof(out), "%s: DLCI %d %s ",
		       header, address, cmd_rsp ? "cmd" : "resp");
	if (ret >= sizeof(out)) {
		LOG_DBG("%d: Too long msg (%d)", __LINE__, ret - sizeof(out));
		goto print;
	}

	frame_type = get_frame_type_str(control & ~PF);
	if (frame_type) {
		ret += snprintk(out + ret, sizeof(out) - ret, "%s ",
				frame_type);
	} else if (!(control & 0x01)) {
		ret += snprintk(out + ret, sizeof(out) - ret,
				"I N(S)%d N(R)%d ",
				(control & 0x0E) >> 1,
				(control & 0xE0) >> 5);
	} else {
		frame_type = get_frame_type_str(control & 0x0F);
		if (frame_type) {
			ret += snprintk(out + ret, sizeof(out) - ret,
					"%s(%d) ", frame_type,
					(control & 0xE0) >> 5);
		} else {
			ret += snprintk(out + ret, sizeof(out) - ret,
					"[%02X] ", control);
		}
	}

	if (ret >= sizeof(out)) {
		LOG_DBG("%d: Too long msg (%d)", __LINE__, ret - sizeof(out));
		goto print;
	}

	ret += snprintk(out + ret, sizeof(out) - ret, "%s",
			(control & PF) ? "(P)" : "(F)");
	if (ret >= sizeof(out)) {
		LOG_DBG("%d: Too long msg (%d)", __LINE__, ret - sizeof(out));
		goto print;
	}

print:
	if (IS_ENABLED(CONFIG_GSM_MUX_VERBOSE_DEBUG)) {
		if (len > 0) {
			LOG_HEXDUMP_DBG(data, len, log_strdup(out));
		} else {
			LOG_DBG("%s", log_strdup(out));
		}
	} else {
		LOG_DBG("%s", log_strdup(out));
	}
}

static uint8_t gsm_mux_fcs_add_buf(uint8_t fcs, const uint8_t *buf, size_t len)
{
	return crc8(buf, len, FCS_POLYNOMIAL, fcs, true);
}

static uint8_t gsm_mux_fcs_add(uint8_t fcs, uint8_t recv_byte)
{
	return gsm_mux_fcs_add_buf(fcs, &recv_byte, 1);
}

static bool gsm_mux_read_ea(int *value, uint8_t recv_byte)
{
	/* As the value can be larger than one byte, collect the read
	 * bytes to given variable.
	 */
	*value <<= 7;
	*value |= recv_byte >> 1;

	/* When the address has been read fully, the EA bit is 1 */
	return recv_byte & EA;
}

static bool gsm_mux_read_msg_len(struct gsm_mux *mux, uint8_t recv_byte)
{
	int value = mux->msg_len;
	bool ret;

	ret = gsm_mux_read_ea(&value, recv_byte);

	mux->msg_len = value;

	return ret;
}

static struct net_buf *gsm_mux_alloc_buf(k_timeout_t timeout, void *user_data)
{
	struct net_buf *buf;

	ARG_UNUSED(user_data);

	buf = net_buf_alloc(&gsm_mux_pool, timeout);
	if (!buf) {
		LOG_ERR("Cannot allocate buffer");
	}

	return buf;
}

static void hexdump_buf(const char *header, struct net_buf *buf)
{
	if (IS_ENABLED(CONFIG_GSM_MUX_VERBOSE_DEBUG)) {
		while (buf) {
			LOG_HEXDUMP_DBG(buf->data, buf->len, header);
			buf = buf->frags;
		}
	}
}

static int gsm_dlci_process_data(struct gsm_dlci *dlci, bool cmd,
				 struct net_buf *buf)
{
	int len = 0;

	LOG_DBG("[%p] DLCI %d data %s", dlci->mux, dlci->num,
		cmd ? "request" : "response");
	hexdump_buf("buf", buf);

	while (buf) {
		uart_mux_recv(dlci->uart, dlci, buf->data, buf->len);
		len += buf->len;
		buf = buf->frags;
	}

	return len;
}

static struct gsm_dlci *gsm_dlci_get(struct gsm_mux *mux, uint8_t dlci_address)
{
	int i;

	for (i = 0; i < ARRAY_SIZE(dlcis); i++) {
		if (dlcis[i].in_use &&
		    dlcis[i].mux == mux &&
		    dlcis[i].num == dlci_address) {
			return &dlcis[i];
		}
	}

	return NULL;
}

static int gsm_mux_modem_send(struct gsm_mux *mux, const uint8_t *buf, size_t size)
{
	if (mux->uart == NULL) {
		return -ENOENT;
	}

	if (size == 0) {
		return 0;
	}

	return uart_mux_send(mux->uart, buf, size);
}

static int gsm_mux_send_data_msg(struct gsm_mux *mux, bool cmd,
				 struct gsm_dlci *dlci, uint8_t frame_type,
				 const uint8_t *buf, size_t size)
{
	uint8_t hdr[7];
	int pos;
	int ret;

	hdr[0] = SOF_MARKER;
	hdr[1] = (dlci->num << 2) | ((uint8_t)cmd << 1) | EA;
	hdr[2] = frame_type;

	if (size < 128) {
		hdr[3] = (size << 1) | EA;
		pos = 4;
	} else {
		hdr[3] = (size >> 7);
		hdr[4] = (size & 127) << 1;
		pos = 5;
	}

	/* Write the header and data in smaller chunks in order to avoid
	 * allocating a big buffer.
	 */
	(void)gsm_mux_modem_send(mux, &hdr[0], pos);

	if (size > 0) {
		(void)gsm_mux_modem_send(mux, buf, size);
	}

	/* FSC is calculated only for address, type and length fields
	 * for UIH frames
	 */
	hdr[pos] = 0xFF - gsm_mux_fcs_add_buf(FCS_INIT_VALUE, &hdr[1],
					      pos - 1);
	if ((frame_type & ~PF) != FT_UIH) {
		hdr[pos] = gsm_mux_fcs_add_buf(hdr[pos], buf, size);
	}

	hdr[pos + 1] = SOF_MARKER;

	ret = gsm_mux_modem_send(mux, &hdr[pos], 2);

	hexdump_packet("Sending", dlci->num, cmd, frame_type,
		       buf, size);
	return ret;
}

static int gsm_mux_send_control_msg(struct gsm_mux *mux, bool cmd,
				    uint8_t dlci_address, uint8_t frame_type)
{
	uint8_t buf[6];

	buf[0] = SOF_MARKER;
	buf[1] = (dlci_address << 2) | ((uint8_t)cmd << 1) | EA;
	buf[2] = frame_type;
	buf[3] = EA;
	buf[4] = 0xFF - gsm_mux_fcs_add_buf(FCS_INIT_VALUE, buf + 1, 3);
	buf[5] = SOF_MARKER;

	hexdump_packet("Sending", dlci_address, cmd, frame_type,
		       buf, sizeof(buf));

	return gsm_mux_modem_send(mux, buf, sizeof(buf));
}

static int gsm_mux_send_command(struct gsm_mux *mux, uint8_t dlci_address,
				uint8_t frame_type)
{
	return gsm_mux_send_control_msg(mux, true, dlci_address, frame_type);
}

static int gsm_mux_send_response(struct gsm_mux *mux, uint8_t dlci_address,
				 uint8_t frame_type)
{
	return gsm_mux_send_control_msg(mux, false, dlci_address, frame_type);
}

static void dlci_run_timer(uint32_t current_time)
{
	struct gsm_dlci *dlci, *next;
	uint32_t new_timer = UINT_MAX;

	if (k_delayed_work_remaining_get(&t1_timer)) {
		k_delayed_work_cancel(&t1_timer);
	}

	SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&dlci_active_t1_timers,
					  dlci, next, node) {
		uint32_t current_timer = dlci->req_start +
			dlci->mux->t1_timeout_value - current_time;

		new_timer = MIN(current_timer, new_timer);
	}

	if (new_timer != UINT_MAX) {
		k_delayed_work_submit(&t1_timer, K_MSEC(new_timer));
	}
}

static void gsm_dlci_open(struct gsm_dlci *dlci)
{
	LOG_DBG("[%p/%d] DLCI id %d open", dlci, dlci->num, dlci->num);
	dlci->state = GSM_DLCI_OPEN;

	/* Remove this DLCI from pending T1 timers */
	sys_slist_remove(&dlci_active_t1_timers, NULL, &dlci->node);
	dlci_run_timer(k_uptime_get_32());

	if (dlci->command_cb) {
		dlci->command_cb(dlci, true);
	}
}

static void gsm_dlci_close(struct gsm_dlci *dlci)
{
	LOG_DBG("[%p/%d] DLCI id %d closed", dlci, dlci->num, dlci->num);
	dlci->state = GSM_DLCI_CLOSED;

	k_sem_give(&dlci->disconnect_sem);

	/* Remove this DLCI from pending T1 timers */
	sys_slist_remove(&dlci_active_t1_timers, NULL, &dlci->node);
	dlci_run_timer(k_uptime_get_32());

	if (dlci->command_cb) {
		dlci->command_cb(dlci, false);
	}

	if (dlci->num == 0) {
		dlci->mux->refuse_service = true;
	}
}

/* Return true if we need to retry, false otherwise */
static bool handle_t1_timeout(struct gsm_dlci *dlci)
{
	LOG_DBG("[%p/%d] T1 timeout", dlci, dlci->num);

	if (dlci->state == GSM_DLCI_OPENING) {
		dlci->retries--;
		if (dlci->retries) {
			dlci->req_start = k_uptime_get_32();
			(void)gsm_mux_send_command(dlci->mux, dlci->num,
						   FT_SABM | PF);
			return true;
		}

		if (dlci->command_cb) {
			dlci->command_cb(dlci, false);
		}

		if (dlci->num == 0 && dlci->mux->control == (FT_DM | PF)) {
			LOG_DBG("DLCI %d -> ADM mode", dlci->num);
			dlci->mode = GSM_DLCI_MODE_ADM;
			gsm_dlci_open(dlci);
		} else {
			gsm_dlci_close(dlci);
		}
	} else if (dlci->state == GSM_DLCI_CLOSING) {
		dlci->retries--;
		if (dlci->retries) {
			(void)gsm_mux_send_command(dlci->mux, dlci->num,
						   FT_DISC | PF);
			return true;
		}

		gsm_dlci_close(dlci);
	}

	return false;
}

static void dlci_t1_timeout(struct k_work *work)
{
	uint32_t current_time = k_uptime_get_32();
	struct gsm_dlci *entry, *next;
	sys_snode_t *prev_node = NULL;

	ARG_UNUSED(work);

	SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&dlci_active_t1_timers,
					  entry, next, node) {
		if ((int32_t)(entry->req_start +
			    entry->mux->t1_timeout_value - current_time) > 0) {
			prev_node = &entry->node;
			break;
		}

		if (!handle_t1_timeout(entry)) {
			sys_slist_remove(&dlci_active_t1_timers, prev_node,
					 &entry->node);
		}
	}

	dlci_run_timer(current_time);
}

static struct gsm_control_msg *gsm_ctrl_msg_get_free(void)
{
	sys_snode_t *node;

	node = sys_slist_peek_head(&ctrls_free_entries);
	if (!node) {
		return NULL;
	}

	sys_slist_remove(&ctrls_free_entries, NULL, node);

	return CONTAINER_OF(node, struct gsm_control_msg, node);
}

static struct gsm_control_msg *gsm_mux_alloc_control_msg(struct net_buf *buf,
							 uint8_t cmd)
{
	struct gsm_control_msg *msg;

	msg = gsm_ctrl_msg_get_free();
	if (!msg) {
		return NULL;
	}

	msg->buf = buf;
	msg->cmd = cmd;

	return msg;
}

static void ctrl_msg_cleanup(struct gsm_control_msg *entry, bool pending)
{
	if (pending) {
		LOG_DBG("Releasing pending buf %p (ref %d)",
			entry->buf, entry->buf->ref - 1);
		net_buf_unref(entry->buf);
		entry->buf = NULL;
	}
}

/* T2 timeout is for control message retransmits */
static void gsm_mux_t2_timeout(struct k_work *work)
{
	struct gsm_mux *mux = CONTAINER_OF(work, struct gsm_mux, t2_timer);
	uint32_t current_time = k_uptime_get_32();
	struct gsm_control_msg *entry, *next;

	SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&mux->pending_ctrls, entry, next,
					  node) {
		if ((int32_t)(entry->req_start + T2_MSEC - current_time) > 0) {
			break;
		}

		ctrl_msg_cleanup(entry, true);

		sys_slist_remove(&mux->pending_ctrls, NULL, &entry->node);
		sys_slist_append(&ctrls_free_entries, &entry->node);

		entry = NULL;
	}

	if (entry) {
		k_delayed_work_submit(&mux->t2_timer,
				      K_MSEC(entry->req_start + T2_MSEC -
					     current_time));
	}
}

static int gsm_mux_send_control_message(struct gsm_mux *mux, uint8_t dlci_address,
					int cmd, uint8_t *data, size_t data_len)
{
	struct gsm_control_msg *ctrl;
	struct net_buf *buf;

	/* We create a net_buf for the control message so that we can
	 * resend it easily if needed.
	 */
	buf = gsm_mux_alloc_buf(BUF_ALLOC_TIMEOUT, NULL);
	if (!buf) {
		LOG_ERR("[%p] Cannot allocate header", mux);
		return -ENOMEM;
	}

	if (data && data_len > 0) {
		size_t added;

		added = net_buf_append_bytes(buf, data_len, data,
					     BUF_ALLOC_TIMEOUT,
					     gsm_mux_alloc_buf, NULL);
		if (added != data_len) {
			net_buf_unref(buf);
			return -ENOMEM;
		}
	}

	ctrl = gsm_mux_alloc_control_msg(buf, cmd);
	if (!ctrl) {
		net_buf_unref(buf);
		return -ENOMEM;
	}

	sys_slist_append(&mux->pending_ctrls, &ctrl->node);
	ctrl->req_start = k_uptime_get_32();

	/* Let's start the timer if necessary */
	if (!k_delayed_work_remaining_get(&mux->t2_timer)) {
		k_delayed_work_submit(&mux->t2_timer, K_MSEC(T2_MSEC));
	}

	return gsm_mux_modem_send(mux, buf->data, buf->len);
}

static int gsm_dlci_opening_or_closing(struct gsm_dlci *dlci,
				       enum gsm_dlci_state state,
				       int command,
				       dlci_command_cb_t cb)
{
	dlci->retries = dlci->mux->retries;
	dlci->req_start = k_uptime_get_32();
	dlci->state = state;
	dlci->command_cb = cb;

	/* Let's start the timer if necessary */
	if (!k_delayed_work_remaining_get(&t1_timer)) {
		k_delayed_work_submit(&t1_timer,
				      K_MSEC(dlci->mux->t1_timeout_value));
	}

	sys_slist_append(&dlci_active_t1_timers, &dlci->node);

	return gsm_mux_send_command(dlci->mux, dlci->num, command | PF);
}

static int gsm_dlci_closing(struct gsm_dlci *dlci, dlci_command_cb_t cb)
{
	if (dlci->state == GSM_DLCI_CLOSED ||
	    dlci->state == GSM_DLCI_CLOSING) {
		return -EALREADY;
	}

	LOG_DBG("[%p] DLCI %d closing", dlci, dlci->num);

	return gsm_dlci_opening_or_closing(dlci, GSM_DLCI_CLOSING, FT_DISC,
					   cb);
}

static int gsm_dlci_opening(struct gsm_dlci *dlci, dlci_command_cb_t cb)
{
	if (dlci->state == GSM_DLCI_OPEN || dlci->state == GSM_DLCI_OPENING) {
		return -EALREADY;
	}

	LOG_DBG("[%p] DLCI %d opening", dlci, dlci->num);

	return gsm_dlci_opening_or_closing(dlci, GSM_DLCI_OPENING, FT_SABM,
					   cb);
}

int gsm_mux_disconnect(struct gsm_mux *mux, k_timeout_t timeout)
{
	struct gsm_dlci *dlci;

	dlci = gsm_dlci_get(mux, 0);
	if (dlci == NULL) {
		return -ENOENT;
	}

	(void)gsm_mux_send_control_message(dlci->mux, dlci->num,
					   CMD_CLD, NULL, 0);

	k_delayed_work_cancel(&mux->t2_timer);

	(void)gsm_dlci_closing(dlci, NULL);

	return k_sem_take(&dlci->disconnect_sem, timeout);
}

static int gsm_mux_control_reply(struct gsm_dlci *dlci, bool sub_cr,
				 uint8_t sub_cmd, const uint8_t *buf, size_t len)
{
	/* As this is a reply to received command, set the value according
	 * to initiator status. See GSM 07.10 page 17.
	 */
	bool cmd = !dlci->mux->is_initiator;

	return gsm_mux_send_data_msg(dlci->mux, cmd, dlci, FT_UIH | PF,
				     buf, len);
}

static bool get_field(struct net_buf *buf, int *ret_value)
{
	int value = 0;
	uint8_t recv_byte;

	while (buf->len) {
		recv_byte = net_buf_pull_u8(buf);

		if (gsm_mux_read_ea(&value, recv_byte)) {
			*ret_value = value;
			return true;
		}

		if (buf->len == 0) {
			buf = net_buf_frag_del(NULL, buf);
			if (buf == NULL) {
				break;
			}
		}
	}

	return false;
}

static int gsm_mux_msc_reply(struct gsm_dlci *dlci, bool cmd,
			     struct net_buf *buf, size_t len)
{
	uint32_t modem_sig = 0, break_sig = 0;
	int ret;

	ret = get_field(buf, &modem_sig);
	if (!ret) {
		LOG_DBG("[%p] Malformed data", dlci->mux);
		return -EINVAL;
	}

	if (buf->len > 0) {
		ret = get_field(buf, &break_sig);
		if (!ret) {
			LOG_DBG("[%p] Malformed data", dlci->mux);
			return -EINVAL;
		}
	}

	LOG_DBG("Modem signal 0x%02x break signal 0x%02x", modem_sig,
		break_sig);

	/* FIXME to return proper status back */

	return gsm_mux_control_reply(dlci, cmd, CMD_MSC, buf->data, len);
}

static int gsm_mux_control_message(struct gsm_dlci *dlci, struct net_buf *buf)
{
	uint32_t command = 0, len = 0;
	int ret = 0;
	bool cr;

	__ASSERT_NO_MSG(dlci != NULL);

	/* Remove the C/R bit from sub-command */
	cr = buf->data[0] & CR;
	buf->data[0] &= ~CR;

	ret = get_field(buf, &command);
	if (!ret) {
		LOG_DBG("[%p] Malformed data", dlci->mux);
		return -EINVAL;
	}

	ret = get_field(buf, &len);
	if (!ret) {
		LOG_DBG("[%p] Malformed data", dlci->mux);
		return -EINVAL;
	}

	LOG_DBG("[%p] DLCI %d %s 0x%02x len %u", dlci->mux, dlci->num,
		cr ? "cmd" : "rsp", command, len);

	/* buf->data should now point to start of dlci command data */

	switch (command) {
	case CMD_CLD:
		/* Modem closing down */
		dlci->mux->refuse_service = true;
		dlci->refuse_service = true;
		gsm_dlci_closing(dlci, NULL);
		break;

	case CMD_FCOFF:
		/* Do not accept data */
		ret = gsm_mux_control_reply(dlci, cr, CMD_FCOFF, NULL, 0);
		break;

	case CMD_FCON:
		/* Accepting data */
		ret = gsm_mux_control_reply(dlci, cr, CMD_FCON, NULL, 0);
		break;

	case CMD_MSC:
		/* Modem status information */
		/* FIXME: WIP: MSC reply does not work */
		if (0) {
			ret = gsm_mux_msc_reply(dlci, cr, buf, len);
		}

		break;

	case CMD_PSC:
		/* Modem wants to enter power saving state */
		ret = gsm_mux_control_reply(dlci, cr, CMD_PSC, NULL, len);
		break;

	case CMD_RLS:
		/* Out of band error reception for a DLCI */
		break;

	case CMD_TEST:
		/* Send test message back */
		ret = gsm_mux_control_reply(dlci, cr, CMD_TEST,
					    buf->data, len);
		break;

	/* Optional and currently unsupported commands */
	case CMD_PN:	/* Parameter negotiation */
	case CMD_RPN:	/* Remote port negotiation */
	case CMD_SNC:	/* Service negotiation command */
	default:
		/* Reply to bad commands with an NSC */
		buf->data[0] = command | (cr ? CR : 0);
		buf->len = 1;
		ret = gsm_mux_control_reply(dlci, cr, CMD_NSC, buf->data, len);
		break;
	}

	return ret;
}

/* Handle a response to our control message */
static int gsm_mux_control_response(struct gsm_dlci *dlci, struct net_buf *buf)
{
	struct gsm_control_msg *entry, *next;

	SYS_SLIST_FOR_EACH_CONTAINER_SAFE(&dlci->mux->pending_ctrls,
					  entry, next, node) {
		if (dlci->mux->control == entry->cmd) {
			sys_slist_remove(&dlci->mux->pending_ctrls, NULL,
					 &entry->node);
			sys_slist_append(&ctrls_free_entries, &entry->node);
			entry->finished = true;

			if (dlci->command_cb) {
				dlci->command_cb(dlci, true);
			}

			break;
		}
	}

	return 0;
}

static int gsm_dlci_process_command(struct gsm_dlci *dlci, bool cmd,
				    struct net_buf *buf)
{
	int ret;

	LOG_DBG("[%p] DLCI %d control %s", dlci->mux, dlci->num,
		cmd ? "request" : "response");
	hexdump_buf("buf", buf);

	if (cmd) {
		ret = gsm_mux_control_message(dlci, buf);
	} else {
		ret = gsm_mux_control_response(dlci, buf);
	}

	return ret;
}

static void gsm_dlci_free(struct gsm_mux *mux, uint8_t address)
{
	struct gsm_dlci *dlci;
	int i;

	for (i = 0; i < ARRAY_SIZE(dlcis); i++) {
		if (!dlcis[i].in_use) {
			continue;
		}

		dlci = &dlcis[i];

		if (dlci->mux == mux && dlci->num == address) {
			dlci->in_use = false;

			sys_slist_prepend(&dlci_free_entries, &dlci->node);
		}

		break;
	}
}

static struct gsm_dlci *gsm_dlci_get_free(void)
{
	sys_snode_t *node;

	node = sys_slist_peek_head(&dlci_free_entries);
	if (!node) {
		return NULL;
	}

	sys_slist_remove(&dlci_free_entries, NULL, node);

	return CONTAINER_OF(node, struct gsm_dlci, node);
}

static struct gsm_dlci *gsm_dlci_alloc(struct gsm_mux *mux, uint8_t address,
		const struct device *uart,
		gsm_mux_dlci_created_cb_t dlci_created_cb,
		void *user_data)
{
	struct gsm_dlci *dlci;

	dlci = gsm_dlci_get_free();
	if (!dlci) {
		return NULL;
	}

	k_sem_init(&dlci->disconnect_sem, 1, 1);

	dlci->mux = mux;
	dlci->num = address;
	dlci->in_use = true;
	dlci->retries = mux->retries;
	dlci->state = GSM_DLCI_CLOSED;
	dlci->uart = uart;
	dlci->user_data = user_data;
	dlci->dlci_created_cb = dlci_created_cb;

	/* Command channel (0) handling is separated from data */
	if (dlci->num) {
		dlci->handler = gsm_dlci_process_data;
	} else {
		dlci->handler = gsm_dlci_process_command;
	}

	return dlci;
}

static int gsm_mux_process_pkt(struct gsm_mux *mux)
{
	uint8_t dlci_address = mux->address >> 2;
	int ret = 0;
	bool cmd;   /* C/R bit, command (true) / response (false) */
	struct gsm_dlci *dlci;

	/* This function is only called for received packets so if the
	 * command is set, then it means a response if we are initiator.
	 */
	cmd = (mux->address >> 1) & 0x01;

	if (mux->is_initiator) {
		cmd = !cmd;
	}

	hexdump_packet("Received", dlci_address, cmd, mux->control,
		       mux->buf ? mux->buf->data : NULL,
		       mux->buf ? mux->buf->len : 0);

	dlci = gsm_dlci_get(mux, dlci_address);

	/* What to do next */
	switch (mux->control) {
	case FT_SABM | PF:
		if (cmd == false) {
			ret = -ENOENT;
			goto fail;
		}

		if (dlci == NULL) {
			const struct device *uart;

			uart = uart_mux_find(dlci_address);
			if (uart == NULL) {
				ret = -ENOENT;
				goto fail;
			}

			dlci = gsm_dlci_alloc(mux, dlci_address, uart, NULL,
					      NULL);
			if (dlci == NULL) {
				ret = -ENOENT;
				goto fail;
			}
		}

		if (dlci->refuse_service) {
			ret = gsm_mux_send_response(mux, dlci_address, FT_DM);
		} else {
			ret = gsm_mux_send_response(mux, dlci_address, FT_UA);
			gsm_dlci_open(dlci);
		}

		break;

	case FT_DISC | PF:
		if (cmd == false) {
			ret = -ENOENT;
			goto fail;
		}

		if (dlci == NULL || dlci->state == GSM_DLCI_CLOSED) {
			(void)gsm_mux_send_response(mux, dlci_address, FT_DM);
			ret = -ENOENT;
			goto out;
		}

		ret = gsm_mux_send_command(mux, dlci_address, FT_UA);
		gsm_dlci_close(dlci);
		break;

	case FT_UA | PF:
	case FT_UA:
		if (cmd == true || dlci == NULL) {
			ret = -ENOENT;
			goto out;
		}

		switch (dlci->state) {
		case GSM_DLCI_CLOSING:
			gsm_dlci_close(dlci);
			break;
		case GSM_DLCI_OPENING:
			gsm_dlci_open(dlci);
			break;
		default:
			break;
		}

		break;

	case FT_DM | PF:
	case FT_DM:
		if (cmd == true || dlci == NULL) {
			ret = -ENOENT;
			goto fail;
		}

		gsm_dlci_close(dlci);
		break;

	case FT_UI | PF:
	case FT_UI:
	case FT_UIH | PF:
	case FT_UIH:
		if (dlci == NULL || dlci->state != GSM_DLCI_OPEN) {
			(void)gsm_mux_send_command(mux, dlci_address,
						   FT_DM | PF);
			ret = -ENOENT;
			goto out;
		}

		ret = dlci->handler(dlci, cmd, mux->buf);

		if (mux->buf) {
			net_buf_unref(mux->buf);
			mux->buf = NULL;
		}

		break;

	default:
		ret = -EINVAL;
		goto fail;
	}

out:
	return ret;

fail:
	LOG_ERR("Cannot handle command (0x%02x) (%d)", mux->control, ret);
	return ret;
}

static bool is_UI(struct gsm_mux *mux)
{
	return (mux->control & ~PF) == FT_UI;
}

const char *gsm_mux_state_str(enum gsm_mux_state state)
{
#if (CONFIG_GSM_MUX_LOG_LEVEL >= LOG_LEVEL_DBG) || \
					defined(CONFIG_NET_SHELL)
	switch (state) {
	case GSM_MUX_SOF:
		return "Start-Of-Frame";
	case GSM_MUX_ADDRESS:
		return "Address";
	case GSM_MUX_CONTROL:
		return "Control";
	case GSM_MUX_LEN_0:
		return "Len0";
	case GSM_MUX_LEN_1:
		return "Len1";
	case GSM_MUX_DATA:
		return "Data";
	case GSM_MUX_FCS:
		return "FCS";
	case GSM_MUX_EOF:
		return "End-Of-Frame";
	}
#else
	ARG_UNUSED(state);
#endif

	return "";
}

#if CONFIG_GSM_MUX_LOG_LEVEL >= LOG_LEVEL_DBG
static void validate_state_transition(enum gsm_mux_state current,
				      enum gsm_mux_state new)
{
	static const uint8_t valid_transitions[] = {
		[GSM_MUX_SOF] = 1 << GSM_MUX_ADDRESS,
		[GSM_MUX_ADDRESS] = 1 << GSM_MUX_CONTROL,
		[GSM_MUX_CONTROL] = 1 << GSM_MUX_LEN_0,
		[GSM_MUX_LEN_0] = 1 << GSM_MUX_LEN_1 |
				1 << GSM_MUX_DATA |
				1 << GSM_MUX_FCS |
				1 << GSM_MUX_SOF,
		[GSM_MUX_LEN_1] = 1 << GSM_MUX_DATA |
				1 << GSM_MUX_FCS |
				1 << GSM_MUX_SOF,
		[GSM_MUX_DATA] = 1 << GSM_MUX_FCS |
				1 << GSM_MUX_SOF,
		[GSM_MUX_FCS] = 1 << GSM_MUX_EOF,
		[GSM_MUX_EOF] = 1 << GSM_MUX_SOF
	};

	if (!(valid_transitions[current] & 1 << new)) {
		LOG_DBG("Invalid state transition: %s (%d) => %s (%d)",
			gsm_mux_state_str(current), current,
			gsm_mux_state_str(new), new);
	}
}
#else
static inline void validate_state_transition(enum gsm_mux_state current,
					     enum gsm_mux_state new)
{
	ARG_UNUSED(current);
	ARG_UNUSED(new);
}
#endif

static inline enum gsm_mux_state gsm_mux_get_state(const struct gsm_mux *mux)
{
	return (enum gsm_mux_state)mux->state;
}

void gsm_mux_change_state(struct gsm_mux *mux, enum gsm_mux_state new_state)
{
	__ASSERT_NO_MSG(mux);

	if (gsm_mux_get_state(mux) == new_state) {
		return;
	}

	LOG_DBG("[%p] state %s (%d) => %s (%d)",
		mux, gsm_mux_state_str(mux->state), mux->state,
		gsm_mux_state_str(new_state), new_state);

	validate_state_transition(mux->state, new_state);

	mux->state = new_state;
}

static void gsm_mux_process_data(struct gsm_mux *mux, uint8_t recv_byte)
{
	size_t bytes_added;

	switch (mux->state) {
	case GSM_MUX_SOF:
		/* This is the initial state where we look for SOF char */
		if (recv_byte == SOF_MARKER) {
			gsm_mux_change_state(mux, GSM_MUX_ADDRESS);
			mux->fcs = FCS_INIT_VALUE;
			mux->received = 0;

			/* Avoid memory leak by freeing all the allocated
			 * buffers at start.
			 */
			if (mux->buf) {
				net_buf_unref(mux->buf);
				mux->buf = NULL;
			}
		}

		break;

	case GSM_MUX_ADDRESS:
		/* DLCI (Data Link Connection Identifier) address we want to
		 * talk. This address field also contains C/R bit.
		 * Currently we only support one byte addresses.
		 */
		mux->address = recv_byte;
		LOG_DBG("[%p] recv %d address %d C/R %d", mux, recv_byte,
			mux->address >> 2, !!(mux->address & CR));
		gsm_mux_change_state(mux, GSM_MUX_CONTROL);
		mux->fcs = gsm_mux_fcs_add(mux->fcs, recv_byte);
		break;

	case GSM_MUX_CONTROL:
		mux->control = recv_byte;
		LOG_DBG("[%p] recv %s (0x%02x) control 0x%02x P/F %d", mux,
			get_frame_type_str(recv_byte & ~PF), recv_byte,
			mux->control & ~PF, !!(mux->control & PF));
		gsm_mux_change_state(mux, GSM_MUX_LEN_0);
		mux->fcs = gsm_mux_fcs_add(mux->fcs, recv_byte);
		break;

	case GSM_MUX_LEN_0:
		mux->fcs = gsm_mux_fcs_add(mux->fcs, recv_byte);
		mux->msg_len = 0;

		if (gsm_mux_read_msg_len(mux, recv_byte)) {
			if (mux->msg_len > mux->mru) {
				gsm_mux_change_state(mux, GSM_MUX_SOF);
			} else if (mux->msg_len == 0) {
				gsm_mux_change_state(mux, GSM_MUX_FCS);
			} else {
				gsm_mux_change_state(mux, GSM_MUX_DATA);

				LOG_DBG("[%p] data len %d", mux, mux->msg_len);
			}
		} else {
			gsm_mux_change_state(mux, GSM_MUX_LEN_1);
		}

		break;

	case GSM_MUX_LEN_1:
		mux->fcs = gsm_mux_fcs_add(mux->fcs, recv_byte);

		mux->msg_len |= recv_byte << 7;
		if (mux->msg_len > mux->mru) {
			gsm_mux_change_state(mux, GSM_MUX_SOF);
		} else if (mux->msg_len == 0) {
			gsm_mux_change_state(mux, GSM_MUX_FCS);
		} else {
			gsm_mux_change_state(mux, GSM_MUX_DATA);

			LOG_DBG("[%p] data len %d", mux, mux->msg_len);
		}

		break;

	case GSM_MUX_DATA:
		if (mux->buf == NULL) {
			mux->buf = net_buf_alloc(&gsm_mux_pool,
						 BUF_ALLOC_TIMEOUT);
			if (mux->buf == NULL) {
				LOG_ERR("[%p] Can't allocate RX data! "
					"Skipping data!", mux);
				gsm_mux_change_state(mux, GSM_MUX_SOF);
				break;
			}
		}

		bytes_added = net_buf_append_bytes(mux->buf, 1,
						   (void *)&recv_byte,
						   BUF_ALLOC_TIMEOUT,
						   gsm_mux_alloc_buf,
						   &gsm_mux_pool);
		if (bytes_added != 1) {
			gsm_mux_change_state(mux, GSM_MUX_SOF);
		} else if (++mux->received == mux->msg_len) {
			gsm_mux_change_state(mux, GSM_MUX_FCS);
		}

		break;

	case GSM_MUX_FCS:
		mux->received_fcs = recv_byte;

		/* Update the FCS for Unnumbered Information field (UI) */
		if (is_UI(mux)) {
			struct net_buf *buf = mux->buf;

			while (buf) {
				mux->fcs = gsm_mux_fcs_add_buf(mux->fcs,
							       buf->data,
							       buf->len);
				buf = buf->frags;
			}
		}

		mux->fcs = gsm_mux_fcs_add(mux->fcs, mux->received_fcs);
		if (mux->fcs == FCS_GOOD_VALUE) {
			int ret = gsm_mux_process_pkt(mux);

			if (ret < 0) {
				LOG_DBG("[%p] Cannot process pkt (%d)", mux,
					ret);
			}
		}

		gsm_mux_change_state(mux, GSM_MUX_EOF);
		break;

	case GSM_MUX_EOF:
		if (recv_byte == SOF_MARKER) {
			gsm_mux_change_state(mux, GSM_MUX_SOF);
		}

		break;
	}
}

void gsm_mux_recv_buf(struct gsm_mux *mux, uint8_t *buf, int len)
{
	int i = 0;

	LOG_DBG("Received %d bytes", len);

	while (i < len) {
		gsm_mux_process_data(mux, buf[i++]);
	}
}

static void dlci_done(struct gsm_dlci *dlci, bool connected)
{
	LOG_DBG("[%p] DLCI id %d %screated", dlci, dlci->num,
		connected == false ? "not " : "");

	/* Let the UART mux to continue */
	if (dlci->dlci_created_cb) {
		dlci->dlci_created_cb(dlci, connected, dlci->user_data);
	}
}

int gsm_dlci_create(struct gsm_mux *mux,
		    const struct device *uart,
		    int dlci_address,
		    gsm_mux_dlci_created_cb_t dlci_created_cb,
		    void *user_data,
		    struct gsm_dlci **dlci)
{
	int ret;

	*dlci = gsm_dlci_alloc(mux, dlci_address, uart, dlci_created_cb,
			       user_data);
	if (!*dlci) {
		LOG_ERR("[%p] Cannot allocate DLCI %d", mux, dlci_address);
		ret = -ENOMEM;
		goto fail;
	}

	ret = gsm_dlci_opening(*dlci, dlci_done);
	if (ret < 0 && ret != -EALREADY) {
		LOG_ERR("[%p] Cannot open DLCI %d", mux, dlci_address);
		gsm_dlci_free(mux, dlci_address);
		*dlci = NULL;
	} else {
		ret = 0;
	}

fail:
	return ret;
}

int gsm_dlci_send(struct gsm_dlci *dlci, const uint8_t *buf, size_t size)
{
	/* Mux the data and send to UART */
	return gsm_mux_send_data_msg(dlci->mux, true, dlci, FT_UIH, buf, size);
}

int gsm_dlci_id(struct gsm_dlci *dlci)
{
	return dlci->num;
}

struct gsm_mux *gsm_mux_create(const struct device *uart)
{
	struct gsm_mux *mux = NULL;
	int i;

	if (!gsm_mux_init_done) {
		LOG_ERR("GSM mux not initialized!");
		return NULL;
	}

	for (i = 0; i < ARRAY_SIZE(muxes); i++) {
		if (muxes[i].in_use) {
			/* If the mux was already created, return it */
			if (uart && muxes[i].uart == uart) {
				return &muxes[i];
			}

			continue;
		}

		mux = &muxes[i];

		memset(mux, 0, sizeof(*mux));

		mux->in_use = true;
		mux->uart = uart;
		mux->mru = CONFIG_GSM_MUX_MRU_DEFAULT_LEN;
		mux->retries = N2;
		mux->t1_timeout_value = CONFIG_GSM_MUX_T1_TIMEOUT ?
			CONFIG_GSM_MUX_T1_TIMEOUT : T1_MSEC;
		mux->t2_timeout_value = T2_MSEC;
		mux->is_initiator = CONFIG_GSM_MUX_INITIATOR;
		mux->state = GSM_MUX_SOF;
		mux->buf = NULL;

		k_delayed_work_init(&mux->t2_timer, gsm_mux_t2_timeout);
		sys_slist_init(&mux->pending_ctrls);

		/* The system will continue after the control DLCI is
		 * created or timeout occurs.
		 */
		break;
	}

	return mux;
}

int gsm_mux_send(struct gsm_mux *mux, uint8_t dlci_address,
		 const uint8_t *buf, size_t size)
{
	struct gsm_dlci *dlci;

	dlci = gsm_dlci_get(mux, dlci_address);
	if (!dlci) {
		return -ENOENT;
	}

	/* Mux the data and send to UART */
	return gsm_mux_send_data_msg(mux, true, dlci, FT_UIH, buf, size);
}

void gsm_mux_init(void)
{
	int i;

	if (gsm_mux_init_done) {
		return;
	}

	gsm_mux_init_done = true;

	sys_slist_init(&ctrls_free_entries);

	for (i = 0; i < ARRAY_SIZE(ctrls); i++) {
		sys_slist_prepend(&ctrls_free_entries, &ctrls[i].node);
	}

	sys_slist_init(&dlci_free_entries);

	for (i = 0; i < ARRAY_SIZE(dlcis); i++) {
		sys_slist_prepend(&dlci_free_entries, &dlcis[i].node);
	}

	k_delayed_work_init(&t1_timer, dlci_t1_timeout);
}