zephyr/drivers/modem/quectel-bg9x.c

/*
 * Copyright (c) 2020 Analog Life LLC
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#define DT_DRV_COMPAT quectel_bg9x

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(modem_quectel_bg9x, CONFIG_MODEM_LOG_LEVEL);

#include "quectel-bg9x.h"

static struct k_thread	       modem_rx_thread;
static struct k_work_q	       modem_workq;
static struct modem_data       mdata;
static struct modem_context    mctx;
static const struct socket_op_vtable offload_socket_fd_op_vtable;

static K_KERNEL_STACK_DEFINE(modem_rx_stack, CONFIG_MODEM_QUECTEL_BG9X_RX_STACK_SIZE);
static K_KERNEL_STACK_DEFINE(modem_workq_stack, CONFIG_MODEM_QUECTEL_BG9X_RX_WORKQ_STACK_SIZE);
NET_BUF_POOL_DEFINE(mdm_recv_pool, MDM_RECV_MAX_BUF, MDM_RECV_BUF_SIZE, 0, NULL);

static const struct gpio_dt_spec power_gpio = GPIO_DT_SPEC_INST_GET(0, mdm_power_gpios);
#if DT_INST_NODE_HAS_PROP(0, mdm_reset_gpios)
static const struct gpio_dt_spec reset_gpio = GPIO_DT_SPEC_INST_GET(0, mdm_reset_gpios);
#endif
#if DT_INST_NODE_HAS_PROP(0, mdm_dtr_gpios)
static const struct gpio_dt_spec dtr_gpio = GPIO_DT_SPEC_INST_GET(0, mdm_dtr_gpios);
#endif
#if DT_INST_NODE_HAS_PROP(0, mdm_wdisable_gpios)
static const struct gpio_dt_spec wdisable_gpio = GPIO_DT_SPEC_INST_GET(0, mdm_wdisable_gpios);
#endif

static inline int digits(int n)
{
	int count = 0;

	while (n != 0) {
		n /= 10;
		++count;
	}

	return count;
}

static inline uint32_t hash32(char *str, int len)
{
#define HASH_MULTIPLIER		37

	uint32_t h = 0;
	int i;

	for (i = 0; i < len; ++i) {
		h = (h * HASH_MULTIPLIER) + str[i];
	}

	return h;
}

static inline uint8_t *modem_get_mac(const struct device *dev)
{
	struct modem_data *data = dev->data;
	uint32_t hash_value;

	data->mac_addr[0] = 0x00;
	data->mac_addr[1] = 0x10;

	/* use IMEI for mac_addr */
	hash_value = hash32(mdata.mdm_imei, strlen(mdata.mdm_imei));

	UNALIGNED_PUT(hash_value, (uint32_t *)(data->mac_addr + 2));

	return data->mac_addr;
}

/* Func: modem_atoi
 * Desc: Convert string to long integer, but handle errors
 */
static int modem_atoi(const char *s, const int err_value,
		      const char *desc, const char *func)
{
	int   ret;
	char  *endptr;

	ret = (int)strtol(s, &endptr, 10);
	if (!endptr || *endptr != '\0') {
		LOG_ERR("bad %s '%s' in %s", s, desc,
			func);
		return err_value;
	}

	return ret;
}

static inline int find_len(char *data)
{
	char buf[10] = {0};
	int  i;

	for (i = 0; i < 10; i++) {
		if (data[i] == '\r') {
			break;
		}

		buf[i] = data[i];
	}

	return ATOI(buf, 0, "rx_buf");
}

/* Func: on_cmd_sockread_common
 * Desc: Function to successfully read data from the modem on a given socket.
 */
static int on_cmd_sockread_common(int socket_fd,
				  struct modem_cmd_handler_data *data,
				  uint16_t len)
{
	struct modem_socket	 *sock = NULL;
	struct socket_read_data	 *sock_data;
	int ret, i;
	int socket_data_length;
	int bytes_to_skip;

	if (!len) {
		LOG_ERR("Invalid length, Aborting!");
		return -EAGAIN;
	}

	/* Make sure we still have buf data */
	if (!data->rx_buf) {
		LOG_ERR("Incorrect format! Ignoring data!");
		return -EINVAL;
	}

	socket_data_length = find_len(data->rx_buf->data);

	/* No (or not enough) data available on the socket. */
	bytes_to_skip = digits(socket_data_length) + 2 + 4;
	if (socket_data_length <= 0) {
		LOG_ERR("Length problem (%d).  Aborting!", socket_data_length);
		return -EAGAIN;
	}

	/* check to make sure we have all of the data. */
	if (net_buf_frags_len(data->rx_buf) < (socket_data_length + bytes_to_skip)) {
		LOG_DBG("Not enough data -- wait!");
		return -EAGAIN;
	}

	/* Skip "len" and CRLF */
	bytes_to_skip = digits(socket_data_length) + 2;
	for (i = 0; i < bytes_to_skip; i++) {
		net_buf_pull_u8(data->rx_buf);
	}

	if (!data->rx_buf->len) {
		data->rx_buf = net_buf_frag_del(NULL, data->rx_buf);
	}

	sock = modem_socket_from_fd(&mdata.socket_config, socket_fd);
	if (!sock) {
		LOG_ERR("Socket not found! (%d)", socket_fd);
		ret = -EINVAL;
		goto exit;
	}

	sock_data = (struct socket_read_data *)sock->data;
	if (!sock_data) {
		LOG_ERR("Socket data not found! Skip handling (%d)", socket_fd);
		ret = -EINVAL;
		goto exit;
	}

	ret = net_buf_linearize(sock_data->recv_buf, sock_data->recv_buf_len,
				data->rx_buf, 0, (uint16_t)socket_data_length);
	data->rx_buf = net_buf_skip(data->rx_buf, ret);
	sock_data->recv_read_len = ret;
	if (ret != socket_data_length) {
		LOG_ERR("Total copied data is different then received data!"
			" copied:%d vs. received:%d", ret, socket_data_length);
		ret = -EINVAL;
	}

exit:
	/* remove packet from list (ignore errors) */
	(void)modem_socket_packet_size_update(&mdata.socket_config, sock,
					      -socket_data_length);

	/* don't give back semaphore -- OK to follow */
	return ret;
}

/* Func: socket_close
 * Desc: Function to close the given socket descriptor.
 */
static void socket_close(struct modem_socket *sock)
{
	char buf[sizeof("AT+QICLOSE=##")] = {0};
	int  ret;

	snprintk(buf, sizeof(buf), "AT+QICLOSE=%d", sock->id);

	/* Tell the modem to close the socket. */
	ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler,
			     NULL, 0U, buf,
			     &mdata.sem_response, MDM_CMD_TIMEOUT);
	if (ret < 0) {
		LOG_ERR("%s ret:%d", buf, ret);
	}

	modem_socket_put(&mdata.socket_config, sock->sock_fd);
}

/* Handler: OK */
MODEM_CMD_DEFINE(on_cmd_ok)
{
	modem_cmd_handler_set_error(data, 0);
	k_sem_give(&mdata.sem_response);
	return 0;
}

/* Handler: ERROR */
MODEM_CMD_DEFINE(on_cmd_error)
{
	modem_cmd_handler_set_error(data, -EIO);
	k_sem_give(&mdata.sem_response);
	return 0;
}

/* Handler: +CME Error: <err>[0] */
MODEM_CMD_DEFINE(on_cmd_exterror)
{
	modem_cmd_handler_set_error(data, -EIO);
	k_sem_give(&mdata.sem_response);
	return 0;
}

/* Handler: +CSQ: <signal_power>[0], <qual>[1] */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_rssi_csq)
{
	int rssi = ATOI(argv[0], 0, "signal_power");

	/* Check the RSSI value. */
	if (rssi == 31) {
		mdata.mdm_rssi = -51;
	} else if (rssi >= 0 && rssi <= 31) {
		mdata.mdm_rssi = -114 + ((rssi * 2) + 1);
	} else {
		mdata.mdm_rssi = -1000;
	}

	LOG_INF("RSSI: %d", mdata.mdm_rssi);
	return 0;
}

/* Handler: +QIOPEN: <connect_id>[0], <err>[1] */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_sockopen)
{
	int err = ATOI(argv[1], 0, "sock_err");

	LOG_INF("AT+QIOPEN: %d", err);
	modem_cmd_handler_set_error(data, err);
	k_sem_give(&mdata.sem_sock_conn);

	return 0;
}

/* Handler: <manufacturer> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_manufacturer)
{
	size_t out_len = net_buf_linearize(mdata.mdm_manufacturer,
					   sizeof(mdata.mdm_manufacturer) - 1,
					   data->rx_buf, 0, len);
	mdata.mdm_manufacturer[out_len] = '\0';
	LOG_INF("Manufacturer: %s", mdata.mdm_manufacturer);
	return 0;
}

/* Handler: <model> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_model)
{
	size_t out_len = net_buf_linearize(mdata.mdm_model,
					   sizeof(mdata.mdm_model) - 1,
					   data->rx_buf, 0, len);
	mdata.mdm_model[out_len] = '\0';

	/* Log the received information. */
	LOG_INF("Model: %s", mdata.mdm_model);
	return 0;
}

/* Handler: <rev> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_revision)
{
	size_t out_len = net_buf_linearize(mdata.mdm_revision,
					   sizeof(mdata.mdm_revision) - 1,
					   data->rx_buf, 0, len);
	mdata.mdm_revision[out_len] = '\0';

	/* Log the received information. */
	LOG_INF("Revision: %s", mdata.mdm_revision);
	return 0;
}

/* Handler: <IMEI> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_imei)
{
	size_t out_len = net_buf_linearize(mdata.mdm_imei,
					   sizeof(mdata.mdm_imei) - 1,
					   data->rx_buf, 0, len);
	mdata.mdm_imei[out_len] = '\0';

	/* Log the received information. */
	LOG_INF("IMEI: %s", mdata.mdm_imei);
	return 0;
}

#if defined(CONFIG_MODEM_SIM_NUMBERS)
/* Handler: <IMSI> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_imsi)
{
	size_t	out_len = net_buf_linearize(mdata.mdm_imsi,
					    sizeof(mdata.mdm_imsi) - 1,
					    data->rx_buf, 0, len);
	mdata.mdm_imsi[out_len] = '\0';

	/* Log the received information. */
	LOG_INF("IMSI: %s", mdata.mdm_imsi);
	return 0;
}

/* Handler: <ICCID> */
MODEM_CMD_DEFINE(on_cmd_atcmdinfo_iccid)
{
	size_t out_len;
	char   *p;

	out_len = net_buf_linearize(mdata.mdm_iccid, sizeof(mdata.mdm_iccid) - 1,
				    data->rx_buf, 0, len);
	mdata.mdm_iccid[out_len] = '\0';

	/* Skip over the +CCID bit, which modems omit. */
	if (mdata.mdm_iccid[0] == '+') {
		p = strchr(mdata.mdm_iccid, ' ');
		if (p) {
			out_len = strlen(p + 1);
			memmove(mdata.mdm_iccid, p + 1, len + 1);
		}
	}

	LOG_INF("ICCID: %s", mdata.mdm_iccid);
	return 0;
}
#endif /* #if defined(CONFIG_MODEM_SIM_NUMBERS) */

/* Handler: TX Ready */
MODEM_CMD_DIRECT_DEFINE(on_cmd_tx_ready)
{
	k_sem_give(&mdata.sem_tx_ready);
	return len;
}

/* Handler: SEND OK */
MODEM_CMD_DEFINE(on_cmd_send_ok)
{
	modem_cmd_handler_set_error(data, 0);
	k_sem_give(&mdata.sem_response);

	return 0;
}

/* Handler: SEND FAIL */
MODEM_CMD_DEFINE(on_cmd_send_fail)
{
	mdata.sock_written = 0;
	modem_cmd_handler_set_error(data, -EIO);
	k_sem_give(&mdata.sem_response);

	return 0;
}

/* Handler: Read data */
MODEM_CMD_DEFINE(on_cmd_sock_readdata)
{
	return on_cmd_sockread_common(mdata.sock_fd, data, len);
}

/* Handler: Data receive indication. */
MODEM_CMD_DEFINE(on_cmd_unsol_recv)
{
	struct modem_socket *sock;
	int		     sock_fd;

	sock_fd = ATOI(argv[0], 0, "sock_fd");

	/* Socket pointer from FD. */
	sock = modem_socket_from_fd(&mdata.socket_config, sock_fd);
	if (!sock) {
		return 0;
	}

	/* Data ready indication. */
	LOG_INF("Data Receive Indication for socket: %d", sock_fd);
	modem_socket_data_ready(&mdata.socket_config, sock);

	return 0;
}

/* Handler: Socket Close Indication. */
MODEM_CMD_DEFINE(on_cmd_unsol_close)
{
	struct modem_socket *sock;
	int		     sock_fd;

	sock_fd = ATOI(argv[0], 0, "sock_fd");
	sock	= modem_socket_from_fd(&mdata.socket_config, sock_fd);
	if (!sock) {
		return 0;
	}

	LOG_INF("Socket Close Indication for socket: %d", sock_fd);

	/* Tell the modem to close the socket. */
	socket_close(sock);
	LOG_INF("Socket Closed: %d", sock_fd);
	return 0;
}

/* Handler: Modem initialization ready. */
MODEM_CMD_DEFINE(on_cmd_unsol_rdy)
{
	k_sem_give(&mdata.sem_response);
	return 0;
}

/* Func: send_socket_data
 * Desc: This function will send "binary" data over the socket object.
 */
static ssize_t send_socket_data(struct modem_socket *sock,
				const struct sockaddr *dst_addr,
				struct modem_cmd *handler_cmds,
				size_t handler_cmds_len,
				const char *buf, size_t buf_len,
				k_timeout_t timeout)
{
	int  ret;
	char send_buf[sizeof("AT+QISEND=##,####")] = {0};
	char ctrlz = 0x1A;

	if (buf_len > MDM_MAX_DATA_LENGTH) {
		buf_len = MDM_MAX_DATA_LENGTH;
	}

	/* Create a buffer with the correct params. */
	mdata.sock_written = buf_len;
	snprintk(send_buf, sizeof(send_buf), "AT+QISEND=%d,%ld", sock->id, (long) buf_len);

	/* Setup the locks correctly. */
	(void)k_sem_take(&mdata.cmd_handler_data.sem_tx_lock, K_FOREVER);
	k_sem_reset(&mdata.sem_tx_ready);

	/* Send the Modem command. */
	ret = modem_cmd_send_nolock(&mctx.iface, &mctx.cmd_handler,
				    NULL, 0U, send_buf, NULL, K_NO_WAIT);
	if (ret < 0) {
		goto exit;
	}

	/* set command handlers */
	ret = modem_cmd_handler_update_cmds(&mdata.cmd_handler_data,
					    handler_cmds, handler_cmds_len,
					    true);
	if (ret < 0) {
		goto exit;
	}

	/* Wait for '>' */
	ret = k_sem_take(&mdata.sem_tx_ready, K_MSEC(5000));
	if (ret < 0) {
		/* Didn't get the data prompt - Exit. */
		LOG_DBG("Timeout waiting for tx");
		goto exit;
	}

	/* Write all data on the console and send CTRL+Z. */
	modem_cmd_send_data_nolock(&mctx.iface, buf, buf_len);
	modem_cmd_send_data_nolock(&mctx.iface, &ctrlz, 1);

	/* Wait for 'SEND OK' or 'SEND FAIL' */
	k_sem_reset(&mdata.sem_response);
	ret = k_sem_take(&mdata.sem_response, timeout);
	if (ret < 0) {
		LOG_DBG("No send response");
		goto exit;
	}

	ret = modem_cmd_handler_get_error(&mdata.cmd_handler_data);
	if (ret != 0) {
		LOG_DBG("Failed to send data");
	}

exit:
	/* unset handler commands and ignore any errors */
	(void)modem_cmd_handler_update_cmds(&mdata.cmd_handler_data,
					    NULL, 0U, false);
	k_sem_give(&mdata.cmd_handler_data.sem_tx_lock);

	if (ret < 0) {
		return ret;
	}

	/* Return the amount of data written on the socket. */
	return mdata.sock_written;
}

/* Func: offload_sendto
 * Desc: This function will send data on the socket object.
 */
static ssize_t offload_sendto(void *obj, const void *buf, size_t len,
			      int flags, const struct sockaddr *to,
			      socklen_t tolen)
{
	int ret;
	struct modem_socket *sock = (struct modem_socket *) obj;

	/* Here's how sending data works,
	 * -> We firstly send the "AT+QISEND" command on the given socket and
	 *    specify the length of data to be transferred.
	 * -> In response to "AT+QISEND" command, the modem may respond with a
	 *    data prompt (>) or not respond at all. If it doesn't respond, we
	 *    exit. If it does respond with a data prompt (>), we move forward.
	 * -> We plainly write all data on the UART and terminate by sending a
	 *    CTRL+Z. Once the modem receives CTRL+Z, it starts processing the
	 *    data and will respond with either "SEND OK", "SEND FAIL" or "ERROR".
	 *    Here we are registering handlers for the first two responses. We
	 *    already have a handler for the "generic" error response.
	 */
	struct modem_cmd cmd[] = {
		MODEM_CMD_DIRECT(">", on_cmd_tx_ready),
		MODEM_CMD("SEND OK", on_cmd_send_ok,   0, ","),
		MODEM_CMD("SEND FAIL", on_cmd_send_fail, 0, ","),
	};

	/* Ensure that valid parameters are passed. */
	if (!buf || len == 0) {
		errno = EINVAL;
		return -1;
	}

	/* UDP is not supported. */
	if (sock->ip_proto == IPPROTO_UDP) {
		errno = ENOTSUP;
		return -1;
	}

	if (!sock->is_connected) {
		errno = ENOTCONN;
		return -1;
	}

	ret = send_socket_data(sock, to, cmd, ARRAY_SIZE(cmd), buf, len,
			       MDM_CMD_TIMEOUT);
	if (ret < 0) {
		errno = -ret;
		return -1;
	}

	/* Data was written successfully. */
	errno = 0;
	return ret;
}

/* Func: offload_recvfrom
 * Desc: This function will receive data on the socket object.
 */
static ssize_t offload_recvfrom(void *obj, void *buf, size_t len,
				int flags, struct sockaddr *from,
				socklen_t *fromlen)
{
	struct modem_socket *sock = (struct modem_socket *)obj;
	char   sendbuf[sizeof("AT+QIRD=##,####")] = {0};
	int    ret;
	struct socket_read_data sock_data;

	/* Modem command to read the data. */
	struct modem_cmd data_cmd[] = { MODEM_CMD("+QIRD: ", on_cmd_sock_readdata, 0U, "") };

	if (!buf || len == 0) {
		errno = EINVAL;
		return -1;
	}

	if (flags & ZSOCK_MSG_PEEK) {
		errno = ENOTSUP;
		return -1;
	}

	snprintk(sendbuf, sizeof(sendbuf), "AT+QIRD=%d,%zd", sock->id, len);

	/* Socket read settings */
	(void) memset(&sock_data, 0, sizeof(sock_data));
	sock_data.recv_buf     = buf;
	sock_data.recv_buf_len = len;
	sock_data.recv_addr    = from;
	sock->data	       = &sock_data;
	mdata.sock_fd	       = sock->sock_fd;

	/* Tell the modem to give us data (AT+QIRD=id,data_len). */
	ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler,
			     data_cmd, ARRAY_SIZE(data_cmd), sendbuf, &mdata.sem_response,
			     MDM_CMD_TIMEOUT);
	if (ret < 0) {
		errno = -ret;
		ret = -1;
		goto exit;
	}

	/* HACK: use dst address as from */
	if (from && fromlen) {
		*fromlen = sizeof(sock->dst);
		memcpy(from, &sock->dst, *fromlen);
	}

	/* return length of received data */
	errno = 0;
	ret = sock_data.recv_read_len;

exit:
	/* clear socket data */
	sock->data = NULL;
	return ret;
}

/* Func: offload_read
 * Desc: This function reads data from the given socket object.
 */
static ssize_t offload_read(void *obj, void *buffer, size_t count)
{
	return offload_recvfrom(obj, buffer, count, 0, NULL, 0);
}

/* Func: offload_write
 * Desc: This function writes data to the given socket object.
 */
static ssize_t offload_write(void *obj, const void *buffer, size_t count)
{
	return offload_sendto(obj, buffer, count, 0, NULL, 0);
}

/* Func: offload_ioctl
 * Desc: Function call to handle various misc requests.
 */
static int offload_ioctl(void *obj, unsigned int request, va_list args)
{
	switch (request) {
	case ZFD_IOCTL_POLL_PREPARE: {
		struct zsock_pollfd *pfd;
		struct k_poll_event **pev;
		struct k_poll_event *pev_end;

		pfd = va_arg(args, struct zsock_pollfd *);
		pev = va_arg(args, struct k_poll_event **);
		pev_end = va_arg(args, struct k_poll_event *);

		return modem_socket_poll_prepare(&mdata.socket_config, obj, pfd, pev, pev_end);
	}
	case ZFD_IOCTL_POLL_UPDATE: {
		struct zsock_pollfd *pfd;
		struct k_poll_event **pev;

		pfd = va_arg(args, struct zsock_pollfd *);
		pev = va_arg(args, struct k_poll_event **);

		return modem_socket_poll_update(obj, pfd, pev);
	}

	default:
		errno = EINVAL;
		return -1;
	}
}

/* Func: offload_connect
 * Desc: This function will connect with a provided TCP.
 */
static int offload_connect(void *obj, const struct sockaddr *addr,
						   socklen_t addrlen)
{
	struct modem_socket *sock     = (struct modem_socket *) obj;
	uint16_t	    dst_port  = 0;
	char		    *protocol = "TCP";
	struct modem_cmd    cmd[]     = { MODEM_CMD("+QIOPEN: ", on_cmd_atcmdinfo_sockopen, 2U, ",") };
	char		    buf[sizeof("AT+QIOPEN=#,#,'###','###',"
				       "####.####.####.####.####.####.####.####,######,"
				       "0,0")] = {0};
	int		    ret;
	char		    ip_str[NET_IPV6_ADDR_LEN];

	/* Verify socket has been allocated */
	if (modem_socket_is_allocated(&mdata.socket_config, sock) == false) {
		LOG_ERR("Invalid socket_id(%d) from fd:%d",
			sock->id, sock->sock_fd);
		errno = EINVAL;
		return -1;
	}

	if (sock->is_connected == true) {
		LOG_ERR("Socket is already connected!! socket_id(%d), socket_fd:%d",
			sock->id, sock->sock_fd);
		errno = EISCONN;
		return -1;
	}

	/* Find the correct destination port. */
	if (addr->sa_family == AF_INET6) {
		dst_port = ntohs(net_sin6(addr)->sin6_port);
	} else if (addr->sa_family == AF_INET) {
		dst_port = ntohs(net_sin(addr)->sin_port);
	}

	/* UDP is not supported. */
	if (sock->ip_proto == IPPROTO_UDP) {
		errno = ENOTSUP;
		return -1;
	}

	k_sem_reset(&mdata.sem_sock_conn);

	ret = modem_context_sprint_ip_addr(addr, ip_str, sizeof(ip_str));
	if (ret != 0) {
		LOG_ERR("Error formatting IP string %d", ret);
		LOG_ERR("Closing the socket!!!");
		socket_close(sock);
		errno = -ret;
		return -1;
	}

	/* Formulate the complete string. */
	snprintk(buf, sizeof(buf), "AT+QIOPEN=%d,%d,\"%s\",\"%s\",%d,0,0", 1, sock->id, protocol,
		 ip_str, dst_port);

	/* Send out the command. */
	ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler,
			     NULL, 0U, buf,
			     &mdata.sem_response, K_SECONDS(1));
	if (ret < 0) {
		LOG_ERR("%s ret:%d", buf, ret);
		LOG_ERR("Closing the socket!!!");
		socket_close(sock);
		errno = -ret;
		return -1;
	}

	/* set command handlers */
	ret = modem_cmd_handler_update_cmds(&mdata.cmd_handler_data,
					    cmd, ARRAY_SIZE(cmd), true);
	if (ret < 0) {
		goto exit;
	}

	/* Wait for QI+OPEN */
	ret = k_sem_take(&mdata.sem_sock_conn, MDM_CMD_CONN_TIMEOUT);
	if (ret < 0) {
		LOG_ERR("Timeout waiting for socket open");
		LOG_ERR("Closing the socket!!!");
		socket_close(sock);
		goto exit;
	}

	ret = modem_cmd_handler_get_error(&mdata.cmd_handler_data);
	if (ret != 0) {
		LOG_ERR("Closing the socket!!!");
		socket_close(sock);
		goto exit;
	}

	/* Connected successfully. */
	sock->is_connected = true;
	errno = 0;
	return 0;

exit:
	(void) modem_cmd_handler_update_cmds(&mdata.cmd_handler_data,
					     NULL, 0U, false);
	errno = -ret;
	return -1;
}

/* Func: offload_close
 * Desc: This function closes the connection with the remote client and
 * frees the socket.
 */
static int offload_close(void *obj)
{
	struct modem_socket *sock = (struct modem_socket *) obj;

	/* Make sure socket is allocated */
	if (modem_socket_is_allocated(&mdata.socket_config, sock) == false) {
		return 0;
	}

	/* Close the socket only if it is connected. */
	if (sock->is_connected) {
		socket_close(sock);
	}

	return 0;
}

/* Func: offload_sendmsg
 * Desc: This function sends messages to the modem.
 */
static ssize_t offload_sendmsg(void *obj, const struct msghdr *msg, int flags)
{
	ssize_t sent = 0;
	int rc;

	LOG_DBG("msg_iovlen:%zd flags:%d", msg->msg_iovlen, flags);

	for (int i = 0; i < msg->msg_iovlen; i++) {
		const char *buf = msg->msg_iov[i].iov_base;
		size_t len	= msg->msg_iov[i].iov_len;

		while (len > 0) {
			rc = offload_sendto(obj, buf, len, flags,
					    msg->msg_name, msg->msg_namelen);
			if (rc < 0) {
				if (rc == -EAGAIN) {
					k_sleep(MDM_SENDMSG_SLEEP);
				} else {
					sent = rc;
					break;
				}
			} else {
				sent += rc;
				buf += rc;
				len -= rc;
			}
		}
	}

	return (ssize_t) sent;
}

/* Func: modem_rx
 * Desc: Thread to process all messages received from the Modem.
 */
static void modem_rx(void *p1, void *p2, void *p3)
{
	ARG_UNUSED(p1);
	ARG_UNUSED(p2);
	ARG_UNUSED(p3);

	while (true) {

		/* Wait for incoming data */
		modem_iface_uart_rx_wait(&mctx.iface, K_FOREVER);

		modem_cmd_handler_process(&mctx.cmd_handler, &mctx.iface);
	}
}

/* Func: modem_rssi_query_work
 * Desc: Routine to get Modem RSSI.
 */
static void modem_rssi_query_work(struct k_work *work)
{
	struct modem_cmd cmd  = MODEM_CMD("+CSQ: ", on_cmd_atcmdinfo_rssi_csq, 2U, ",");
	static char *send_cmd = "AT+CSQ";
	int ret;

	/* query modem RSSI */
	ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler,
			     &cmd, 1U, send_cmd, &mdata.sem_response,
			     MDM_CMD_TIMEOUT);
	if (ret < 0) {
		LOG_ERR("AT+CSQ ret:%d", ret);
	}

	/* Re-start RSSI query work */
	if (work) {
		k_work_reschedule_for_queue(&modem_workq,
					    &mdata.rssi_query_work,
					    K_SECONDS(RSSI_TIMEOUT_SECS));
	}
}

/* Func: pin_init
 * Desc: Boot up the Modem.
 */
static void pin_init(void)
{
#if !DT_INST_NODE_HAS_PROP(0, mdm_reset_gpios)
	int ret = k_sem_take(&mdata.sem_pin_busy, K_SECONDS(3));

	if (ret < 0) {
		LOG_DBG("Timeout pin_init()");
	}
#endif /* !DT_INST_NODE_HAS_PROP(0, mdm_reset_gpios) */
	LOG_INF("Setting Modem Pins");

#if DT_INST_NODE_HAS_PROP(0, mdm_wdisable_gpios)
	LOG_INF("Deactivate W Disable");
	gpio_pin_set_dt(&wdisable_gpio, 0);
	k_sleep(K_MSEC(250));
#endif

	/* NOTE: Per the BG95 document, the Reset pin is internally connected to the
	 * Power key pin.
	 */

	/* MDM_POWER -> 1 for 500-1000 msec. */
	gpio_pin_set_dt(&power_gpio, 1);
	k_sleep(K_MSEC(750));

	/* MDM_POWER -> 0 and wait for ~2secs as UART remains in "inactive" state
	 * for some time after the power signal is enabled.
	 */
	gpio_pin_set_dt(&power_gpio, 0);
	k_sleep(K_SECONDS(2));

	LOG_INF("... Done!");

#if !DT_INST_NODE_HAS_PROP(0, mdm_reset_gpios)
	k_sem_give(&mdata.sem_pin_busy);
#endif /* !DT_INST_NODE_HAS_PROP(0, mdm_reset_gpios) */
}

MODEM_CMD_DEFINE(on_cmd_unsol_normal_power_down)
{
	LOG_INF("Modem powering off. Re-power modem...");
	pin_init();

	return 0;
}

static const struct modem_cmd response_cmds[] = {
	MODEM_CMD("OK", on_cmd_ok, 0U, ""),
	MODEM_CMD("ERROR", on_cmd_error, 0U, ""),
	MODEM_CMD("+CME ERROR: ", on_cmd_exterror, 1U, ""),
};

static const struct modem_cmd unsol_cmds[] = {
	MODEM_CMD("+QIURC: \"recv\",",	   on_cmd_unsol_recv,  1U, ""),
	MODEM_CMD("+QIURC: \"closed\",",   on_cmd_unsol_close, 1U, ""),
	MODEM_CMD(MDM_UNSOL_RDY, on_cmd_unsol_rdy, 0U, ""),
	MODEM_CMD("NORMAL POWER DOWN", on_cmd_unsol_normal_power_down, 0U, ""),
};

/* Commands sent to the modem to set it up at boot time. */
static const struct setup_cmd setup_cmds[] = {
	SETUP_CMD_NOHANDLE("ATE0"),
	SETUP_CMD_NOHANDLE("ATH"),
	SETUP_CMD_NOHANDLE("AT+CMEE=1"),

	/* Commands to read info from the modem (things like IMEI, Model etc). */
	SETUP_CMD("AT+CGMI", "", on_cmd_atcmdinfo_manufacturer, 0U, ""),
	SETUP_CMD("AT+CGMM", "", on_cmd_atcmdinfo_model, 0U, ""),
	SETUP_CMD("AT+CGMR", "", on_cmd_atcmdinfo_revision, 0U, ""),
	SETUP_CMD("AT+CGSN", "", on_cmd_atcmdinfo_imei, 0U, ""),
#if defined(CONFIG_MODEM_SIM_NUMBERS)
	SETUP_CMD("AT+CIMI", "", on_cmd_atcmdinfo_imsi, 0U, ""),
	SETUP_CMD("AT+QCCID", "", on_cmd_atcmdinfo_iccid, 0U, ""),
#endif /* #if defined(CONFIG_MODEM_SIM_NUMBERS) */
	SETUP_CMD_NOHANDLE("AT+QICSGP=1,1,\"" MDM_APN "\",\""
			   MDM_USERNAME "\",\"" MDM_PASSWORD "\",1"),
};

/* Func: modem_pdp_context_active
 * Desc: This helper function is called from modem_setup, and is
 * used to open the PDP context. If there is trouble activating the
 * PDP context, we try to deactivate and reactivate MDM_PDP_ACT_RETRY_COUNT times.
 * If it fails, we return an error.
 */
static int modem_pdp_context_activate(void)
{
	int ret;
	int retry_count = 0;

	ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler,
			     NULL, 0U, "AT+QIACT=1", &mdata.sem_response,
			     MDM_CMD_TIMEOUT);

	/* If there is trouble activating the PDP context, we try to deactivate/reactive it. */
	while (ret == -EIO && retry_count < MDM_PDP_ACT_RETRY_COUNT) {
		ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler,
			     NULL, 0U, "AT+QIDEACT=1", &mdata.sem_response,
			     MDM_CMD_TIMEOUT);

		/* If there's any error for AT+QIDEACT, restart the module. */
		if (ret != 0) {
			return ret;
		}

		ret = modem_cmd_send(&mctx.iface, &mctx.cmd_handler,
			     NULL, 0U, "AT+QIACT=1", &mdata.sem_response,
			     MDM_CMD_TIMEOUT);

		retry_count++;
	}

	if (ret == -EIO && retry_count >= MDM_PDP_ACT_RETRY_COUNT) {
		LOG_ERR("Retried activating/deactivating too many times.");
	}

	return ret;
}

/* Func: modem_setup
 * Desc: This function is used to setup the modem from zero. The idea
 * is that this function will be called right after the modem is
 * powered on to do the stuff necessary to talk to the modem.
 */
static int modem_setup(void)
{
	int ret = 0, counter;
	int rssi_retry_count = 0, init_retry_count = 0;

	/* Setup the pins to ensure that Modem is enabled. */
	pin_init();

restart:

	counter = 0;

	/* stop RSSI delay work */
	k_work_cancel_delayable(&mdata.rssi_query_work);

	/* Let the modem respond. */
	LOG_INF("Waiting for modem to respond");
	ret = k_sem_take(&mdata.sem_response, MDM_MAX_BOOT_TIME);
	if (ret < 0) {
		LOG_ERR("Timeout waiting for RDY");
		goto error;
	}

	/* Run setup commands on the modem. */
	ret = modem_cmd_handler_setup_cmds(&mctx.iface, &mctx.cmd_handler,
					   setup_cmds, ARRAY_SIZE(setup_cmds),
					   &mdata.sem_response, MDM_REGISTRATION_TIMEOUT);
	if (ret < 0) {
		goto error;
	}

restart_rssi:

	/* query modem RSSI */
	modem_rssi_query_work(NULL);
	k_sleep(MDM_WAIT_FOR_RSSI_DELAY);

	/* Keep trying to read RSSI until we get a valid value - Eventually, exit. */
	while (counter++ < MDM_WAIT_FOR_RSSI_COUNT &&
	      (mdata.mdm_rssi >= 0 || mdata.mdm_rssi <= -1000)) {
		modem_rssi_query_work(NULL);
		k_sleep(MDM_WAIT_FOR_RSSI_DELAY);
	}

	/* Is the RSSI invalid ? */
	if (mdata.mdm_rssi >= 0 || mdata.mdm_rssi <= -1000) {
		rssi_retry_count++;

		if (rssi_retry_count >= MDM_NETWORK_RETRY_COUNT) {
			LOG_ERR("Failed network init. Too many attempts!");
			ret = -ENETUNREACH;
			goto error;
		}

		/* Try again! */
		LOG_ERR("Failed network init. Restarting process.");
		counter = 0;
		goto restart_rssi;
	}

	/* Network is ready - Start RSSI work in the background. */
	LOG_INF("Network is ready.");
	k_work_reschedule_for_queue(&modem_workq, &mdata.rssi_query_work,
				    K_SECONDS(RSSI_TIMEOUT_SECS));

	/* Once the network is ready, we try to activate the PDP context. */
	ret = modem_pdp_context_activate();
	if (ret < 0 && init_retry_count++ < MDM_INIT_RETRY_COUNT) {
		LOG_ERR("Error activating modem with pdp context");
		goto restart;
	}

error:
	return ret;
}

static const struct socket_op_vtable offload_socket_fd_op_vtable = {
	.fd_vtable = {
		.read	= offload_read,
		.write	= offload_write,
		.close	= offload_close,
		.ioctl	= offload_ioctl,
	},
	.bind		= NULL,
	.connect	= offload_connect,
	.sendto		= offload_sendto,
	.recvfrom	= offload_recvfrom,
	.listen		= NULL,
	.accept		= NULL,
	.sendmsg	= offload_sendmsg,
	.getsockopt	= NULL,
	.setsockopt	= NULL,
};

static int offload_socket(int family, int type, int proto);

/* Setup the Modem NET Interface. */
static void modem_net_iface_init(struct net_if *iface)
{
	const struct device *dev = net_if_get_device(iface);
	struct modem_data *data	 = dev->data;

	/* Direct socket offload used instead of net offload: */
	net_if_set_link_addr(iface, modem_get_mac(dev),
			     sizeof(data->mac_addr),
			     NET_LINK_ETHERNET);
	data->net_iface = iface;

	net_if_socket_offload_set(iface, offload_socket);
}

static struct offloaded_if_api api_funcs = {
	.iface_api.init = modem_net_iface_init,
};

static bool offload_is_supported(int family, int type, int proto)
{
	if (family != AF_INET &&
	    family != AF_INET6) {
		return false;
	}

	if (type != SOCK_STREAM) {
		return false;
	}

	if (proto != IPPROTO_TCP) {
		return false;
	}

	return true;
}

static int offload_socket(int family, int type, int proto)
{
	int ret;

	/* defer modem's socket create call to bind() */
	ret = modem_socket_get(&mdata.socket_config, family, type, proto);
	if (ret < 0) {
		errno = -ret;
		return -1;
	}

	errno = 0;
	return ret;
}

static int modem_init(const struct device *dev)
{
	int ret; ARG_UNUSED(dev);

#if !DT_INST_NODE_HAS_PROP(0, mdm_reset_gpios)
	k_sem_init(&mdata.sem_pin_busy,	 1, 1);
#endif /* !DT_INST_NODE_HAS_PROP(0, mdm_reset_gpios) */
	k_sem_init(&mdata.sem_response,	 0, 1);
	k_sem_init(&mdata.sem_tx_ready,	 0, 1);
	k_sem_init(&mdata.sem_sock_conn, 0, 1);
	k_work_queue_start(&modem_workq, modem_workq_stack,
			   K_KERNEL_STACK_SIZEOF(modem_workq_stack),
			   K_PRIO_COOP(7), NULL);

	/* socket config */
	ret = modem_socket_init(&mdata.socket_config, &mdata.sockets[0], ARRAY_SIZE(mdata.sockets),
				MDM_BASE_SOCKET_NUM, true, &offload_socket_fd_op_vtable);
	if (ret < 0) {
		goto error;
	}

	/* cmd handler setup */
	const struct modem_cmd_handler_config cmd_handler_config = {
		.match_buf = &mdata.cmd_match_buf[0],
		.match_buf_len = sizeof(mdata.cmd_match_buf),
		.buf_pool = &mdm_recv_pool,
		.alloc_timeout = BUF_ALLOC_TIMEOUT,
		.eol = "\r\n",
		.user_data = NULL,
		.response_cmds = response_cmds,
		.response_cmds_len = ARRAY_SIZE(response_cmds),
		.unsol_cmds = unsol_cmds,
		.unsol_cmds_len = ARRAY_SIZE(unsol_cmds),
	};

	ret = modem_cmd_handler_init(&mctx.cmd_handler, &mdata.cmd_handler_data,
				     &cmd_handler_config);
	if (ret < 0) {
		goto error;
	}

	/* modem interface */
	const struct modem_iface_uart_config uart_config = {
		.rx_rb_buf = &mdata.iface_rb_buf[0],
		.rx_rb_buf_len = sizeof(mdata.iface_rb_buf),
		.dev = MDM_UART_DEV,
		.hw_flow_control = DT_PROP(MDM_UART_NODE, hw_flow_control),
	};

	ret = modem_iface_uart_init(&mctx.iface, &mdata.iface_data, &uart_config);
	if (ret < 0) {
		goto error;
	}

	/* modem data storage */
	mctx.data_manufacturer = mdata.mdm_manufacturer;
	mctx.data_model	       = mdata.mdm_model;
	mctx.data_revision     = mdata.mdm_revision;
	mctx.data_imei	       = mdata.mdm_imei;
#if defined(CONFIG_MODEM_SIM_NUMBERS)
	mctx.data_imsi	       = mdata.mdm_imsi;
	mctx.data_iccid	       = mdata.mdm_iccid;
#endif /* #if defined(CONFIG_MODEM_SIM_NUMBERS) */
	mctx.data_rssi = &mdata.mdm_rssi;

	/* pin setup */
	ret = gpio_pin_configure_dt(&power_gpio, GPIO_OUTPUT_LOW);
	if (ret < 0) {
		LOG_ERR("Failed to configure %s pin", "power");
		goto error;
	}

#if DT_INST_NODE_HAS_PROP(0, mdm_reset_gpios)
	ret = gpio_pin_configure_dt(&reset_gpio, GPIO_OUTPUT_LOW);
	if (ret < 0) {
		LOG_ERR("Failed to configure %s pin", "reset");
		goto error;
	}
#endif

#if DT_INST_NODE_HAS_PROP(0, mdm_dtr_gpios)
	ret = gpio_pin_configure_dt(&dtr_gpio, GPIO_OUTPUT_LOW);
	if (ret < 0) {
		LOG_ERR("Failed to configure %s pin", "dtr");
		goto error;
	}
#endif

#if DT_INST_NODE_HAS_PROP(0, mdm_wdisable_gpios)
	ret = gpio_pin_configure_dt(&wdisable_gpio, GPIO_OUTPUT_LOW);
	if (ret < 0) {
		LOG_ERR("Failed to configure %s pin", "wdisable");
		goto error;
	}
#endif

	/* modem context setup */
	mctx.driver_data       = &mdata;

	ret = modem_context_register(&mctx);
	if (ret < 0) {
		LOG_ERR("Error registering modem context: %d", ret);
		goto error;
	}

	/* start RX thread */
	k_thread_create(&modem_rx_thread, modem_rx_stack,
			K_KERNEL_STACK_SIZEOF(modem_rx_stack),
			modem_rx,
			NULL, NULL, NULL, K_PRIO_COOP(7), 0, K_NO_WAIT);

	/* Init RSSI query */
	k_work_init_delayable(&mdata.rssi_query_work, modem_rssi_query_work);
	return modem_setup();

error:
	return ret;
}

/* Register the device with the Networking stack. */
NET_DEVICE_DT_INST_OFFLOAD_DEFINE(0, modem_init, NULL,
				  &mdata, NULL,
				  CONFIG_MODEM_QUECTEL_BG9X_INIT_PRIORITY,
				  &api_funcs, MDM_MAX_DATA_LENGTH);

/* Register NET sockets. */
NET_SOCKET_OFFLOAD_REGISTER(quectel_bg9x, CONFIG_NET_SOCKETS_OFFLOAD_PRIORITY,
			    AF_UNSPEC, offload_is_supported, offload_socket);