zephyr/subsys/usb/class/netusb/function_rndis.c

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

#define SYS_LOG_LEVEL CONFIG_SYS_LOG_USB_DEVICE_NETWORK_DEBUG_LEVEL
#define SYS_LOG_DOMAIN "function/rndis"
#include <logging/sys_log.h>

/* Enable verbose debug printing extra hexdumps */
#define VERBOSE_DEBUG	0

/* This enables basic hexdumps */
#define NET_LOG_ENABLED	0
#include <net_private.h>

#include <zephyr.h>

#include <usb_device.h>
#include <usb_common.h>

#include <net/ethernet.h>
#include <net/net_pkt.h>

#include <class/usb_cdc.h>

#include "netusb.h"
#include "function_rndis.h"

/* RNDIS handling */
#define CONFIG_RNDIS_TX_BUF_COUNT	5
#define CONFIG_RNDIS_TX_BUF_SIZE	512
NET_BUF_POOL_DEFINE(rndis_tx_pool, CONFIG_RNDIS_TX_BUF_COUNT,
		    CONFIG_RNDIS_TX_BUF_SIZE, 0, NULL);
static struct k_fifo rndis_tx_queue;

/* Serialize RNDIS command queue for later processing */
#define CONFIG_RNDIS_CMD_BUF_COUNT	2
#define CONFIG_RNDIS_CMD_BUF_SIZE	512
NET_BUF_POOL_DEFINE(rndis_cmd_pool, CONFIG_RNDIS_CMD_BUF_COUNT,
		    CONFIG_RNDIS_CMD_BUF_SIZE, 0, NULL);
static struct k_fifo rndis_cmd_queue;

static struct k_delayed_work notify_work;

/*
 * Stack for cmd thread
 */
static K_THREAD_STACK_DEFINE(cmd_stack, 2048);
static struct k_thread cmd_thread_data;

/*
 * TLV structure is used for data encapsulation parsing
 */
struct tlv {
	u32_t type;
	u32_t len;
	u8_t data[];
} __packed;

static struct __rndis {
	u32_t net_filter;

	enum {
		UNINITIALIZED,
		INITIALIZED,
	} state;

	struct net_pkt *in_pkt;	/* Pointer to pkt assembling at the moment */
	int in_pkt_len;		/* Packet length to be assembled */
	int skip_bytes;		/* In case of low memory, skip bytes */

	u16_t mtu;
	u16_t speed;		/* TODO: Calculate right speed */

	/* Statistics */
	u32_t rx_err;
	u32_t tx_err;
	u32_t rx_no_buf;

	atomic_t notify_count;

	u8_t mac[6];
	u8_t media_status;
} rndis = {
	.mac =  { 0x00, 0x11, 0x22, 0x33, 0x44, 0x66 },
	.mtu = 1500, /* Ethernet frame */
	.media_status = RNDIS_OBJECT_ID_MEDIA_DISCONNECTED,
	.state = UNINITIALIZED,
	.skip_bytes = 0,
	.speed = 0,
};

static u8_t manufacturer[] = CONFIG_USB_DEVICE_MANUFACTURER;
static u32_t drv_version = 1;

static u32_t object_id_supported[] = {
	RNDIS_OBJECT_ID_GEN_SUPP_LIST,
	RNDIS_OBJECT_ID_GEN_HW_STATUS,
	RNDIS_OBJECT_ID_GEN_SUPP_MEDIA,
	RNDIS_OBJECT_ID_GEN_IN_USE_MEDIA,

	RNDIS_OBJECT_ID_GEN_MAX_FRAME_SIZE,
	RNDIS_OBJECT_ID_GEN_LINK_SPEED,
	RNDIS_OBJECT_ID_GEN_BLOCK_TX_SIZE,
	RNDIS_OBJECT_ID_GEN_BLOCK_RX_SIZE,

	RNDIS_OBJECT_ID_GEN_VENDOR_ID,
	RNDIS_OBJECT_ID_GEN_VENDOR_DESC,
	RNDIS_OBJECT_ID_GEN_VENDOR_DRV_VER,

	RNDIS_OBJECT_ID_GEN_PKT_FILTER,
	RNDIS_OBJECT_ID_GEN_MAX_TOTAL_SIZE,
	RNDIS_OBJECT_ID_GEN_CONN_MEDIA_STATUS,
	RNDIS_OBJECT_ID_GEN_PHYSICAL_MEDIUM,
#if defined(USE_RNDIS_STATISTICS)
	/* Using RNDIS statistics puts heavy load on
	 * USB bus, disable it for now
	 */
	RNDIS_OBJECT_ID_GEN_TRANSMIT_OK,
	RNDIS_OBJECT_ID_GEN_RECEIVE_OK,
	RNDIS_OBJECT_ID_GEN_TRANSMIT_ERROR,
	RNDIS_OBJECT_ID_GEN_RECEIVE_ERROR,
	RNDIS_OBJECT_ID_GEN_RECEIVE_NO_BUF,
#endif /* USE_RNDIS_STATISTICS */
	RNDIS_OBJECT_ID_802_3_PERMANENT_ADDRESS,
	RNDIS_OBJECT_ID_802_3_CURR_ADDRESS,
	RNDIS_OBJECT_ID_802_3_MCAST_LIST,
	RNDIS_OBJECT_ID_802_3_MAX_LIST_SIZE,
	RNDIS_OBJECT_ID_802_3_MAC_OPTIONS,
};

static int parse_rndis_header(const u8_t *buffer, u32_t buf_len)
{
	struct rndis_payload_packet *hdr = (void *)buffer;
	u32_t len;

	if (buf_len < sizeof(*hdr)) {
		SYS_LOG_ERR("Too small packet len %u", buf_len);
		return -EINVAL;
	}

	if (hdr->type != sys_cpu_to_le32(RNDIS_DATA_PACKET)) {
		SYS_LOG_ERR("Wrong data packet type 0x%x",
			    sys_le32_to_cpu(hdr->type));
		return -EINVAL;
	}

	len = sys_le32_to_cpu(hdr->len);
	/*
	 * Calculate additional offset since payload_offset is calculated
	 * from the start of itself ;)
	 */
	if (len < sys_le32_to_cpu(hdr->payload_offset) +
	    sys_le32_to_cpu(hdr->payload_len) +
	    offsetof(struct rndis_payload_packet, payload_offset)) {
		SYS_LOG_ERR("Incorrect RNDIS packet");
		return -EINVAL;
	}

	SYS_LOG_DBG("Parsing packet: len %u payload offset %u payload len %u",
		    len, sys_le32_to_cpu(hdr->payload_offset),
		    sys_le32_to_cpu(hdr->payload_len));

	return len;
}

void rndis_clean(void)
{
	SYS_LOG_DBG("");

	if (rndis.in_pkt) {
		net_pkt_unref(rndis.in_pkt);

		rndis.in_pkt = NULL;
		rndis.in_pkt_len = 0;
	}

	rndis.skip_bytes = 0;
}

static void rndis_bulk_out(u8_t ep, enum usb_dc_ep_cb_status_code ep_status)
{
	u8_t buffer[CONFIG_RNDIS_BULK_EP_MPS];
	u32_t hdr_offset = 0;
	u32_t len, read;
	int ret;

	usb_read(ep, NULL, 0, &len);

	SYS_LOG_DBG("EP 0x%x status %d len %u", ep, ep_status, len);

	if (len > CONFIG_RNDIS_BULK_EP_MPS) {
		SYS_LOG_WRN("Limit read len %u to MPS %u", len,
			    CONFIG_RNDIS_BULK_EP_MPS);
		len = CONFIG_RNDIS_BULK_EP_MPS;
	}

	usb_read(ep, buffer, len, &read);
	if (len != read) {
		SYS_LOG_ERR("Read %u instead of expected %u, skip the rest",
			    read, len);
		rndis.skip_bytes = len - read;
		return;
	}

	/* We already use frame keeping with len, warn here about
	 * receiving frame delimeter
	 */
	if (len == 1 && !buffer[0]) {
		SYS_LOG_DBG("Got frame delimeter, skip");
		return;
	}

	/* Handle skip bytes */
	if (rndis.skip_bytes) {
		SYS_LOG_WRN("Skip %u bytes out of remaining %d bytes",
			    len, rndis.skip_bytes);

		rndis.skip_bytes -= len;

		if (rndis.skip_bytes < 0) {
			SYS_LOG_ERR("Error skipping bytes");

			rndis.skip_bytes = 0;
		}

		return;
	}

	/* Start new packet */
	if (!rndis.in_pkt) {
		struct net_pkt *pkt;
		struct net_buf *buf;

		rndis.in_pkt_len = parse_rndis_header(buffer, len);
		if (rndis.in_pkt_len < 0) {
			SYS_LOG_ERR("Error parsing RNDIS header");

			rndis.rx_err++;
			return;
		}

		pkt = net_pkt_get_reserve_rx(0, K_NO_WAIT);
		if (!pkt) {
			/* In a case of low memory skip the whole packet
			 * hoping to get buffers for later ones
			 */
			rndis.skip_bytes = rndis.in_pkt_len - len;
			rndis.rx_no_buf++;

			SYS_LOG_ERR("Not enough pkt buffers, len %u, skip %u",
				    rndis.in_pkt_len, rndis.skip_bytes);

			return;
		}

		buf = net_pkt_get_frag(pkt, K_NO_WAIT);
		if (!buf) {
			/* In a case of low memory skip the whole packet
			 * hoping to get buffers for later ones
			 */
			rndis.skip_bytes = rndis.in_pkt_len - len;

			SYS_LOG_ERR("Not enough net buffers, len %u, skip %u",
				    rndis.in_pkt_len, rndis.skip_bytes);

			net_pkt_unref(pkt);
			rndis.rx_no_buf++;
			return;
		}

		net_pkt_frag_insert(pkt, buf);

		rndis.in_pkt = pkt;

		/* Append data only, skipping RNDIS header */
		hdr_offset = sizeof(struct rndis_payload_packet);
	}

	ret = net_pkt_append_all(rndis.in_pkt, len - hdr_offset,
				 buffer + hdr_offset, K_FOREVER);
	if (ret < 0) {
		SYS_LOG_ERR("Error appending data to pkt: %p", rndis.in_pkt);
		rndis_clean();
		rndis.rx_err++;
		return;
	}

	SYS_LOG_DBG("To asemble %d bytes, reading %u bytes",
		    rndis.in_pkt_len, len);

	rndis.in_pkt_len -= len;
	if (!rndis.in_pkt_len) {
		SYS_LOG_DBG("Assembled full RNDIS packet");

		net_hexdump_frags(">", rndis.in_pkt, true);

		/* Queue data to iface */
		netusb_recv(rndis.in_pkt);

		/* Start over for new packets */
		rndis.in_pkt = NULL;
	} else if (rndis.in_pkt_len < 0) {
		SYS_LOG_ERR("Error assembling packet, drop and start over");
		rndis_clean();
	}
}

static void rndis_int_in(u8_t ep, enum usb_dc_ep_cb_status_code ep_status)
{
#ifdef VERBOSE_DEBUG
	SYS_LOG_DBG("EP 0x%x status %d", ep, ep_status);
#endif
}

static void rndis_bulk_in(u8_t ep, enum usb_dc_ep_cb_status_code ep_status)
{
#ifdef VERBOSE_DEBUG
	SYS_LOG_DBG("EP 0x%x status %d", ep, ep_status);
#endif
}

static void rndis_notify(struct k_work *work)
{
	u32_t buf[2];

	SYS_LOG_DBG("count %u", atomic_get(&rndis.notify_count));

	buf[0] = sys_cpu_to_le32(0x01);
	buf[1] = sys_cpu_to_le32(0x00);

	try_write(CONFIG_RNDIS_INT_EP_ADDR, (u8_t *)buf, sizeof(buf));

	/* Decrement notify_count here */
	if (atomic_dec(&rndis.notify_count) != 1) {
		SYS_LOG_WRN("Queue next notification, count %u",
			    atomic_get(&rndis.notify_count));

		k_delayed_work_submit(&notify_work, K_NO_WAIT);
	}
}

static void rndis_send_zero_frame(void)
{
	u8_t zero[] = { 0x00 };

	SYS_LOG_DBG("Last packet, send zero frame");

	try_write(CONFIG_RNDIS_IN_EP_ADDR, zero, sizeof(zero));
}

static void rndis_queue_rsp(struct net_buf *rsp)
{

	if (!k_fifo_is_empty(&rndis_tx_queue)) {
#if CLEAN_TX_QUEUE
		struct net_buf *buf;

		while ((buf = net_buf_get(&rndis_tx_queue, K_NO_WAIT))) {
			SYS_LOG_ERR("Drop buffer %p", buf);
			net_buf_unref(buf);
		}
#endif
		SYS_LOG_WRN("Transmit response queue is not empty");
	}

	SYS_LOG_DBG("Queued response pkt %p", rsp);

	net_buf_put(&rndis_tx_queue, rsp);
}

/* Notify host about available data */
static void rndis_notify_rsp(void)
{
	int ret;

	SYS_LOG_DBG("count %u", atomic_get(&rndis.notify_count));

	/* Keep track of number of notifies */
	if (atomic_inc(&rndis.notify_count) != 0) {
		SYS_LOG_WRN("Unhandled notify: count %u",
			    atomic_get(&rndis.notify_count));

		return;
	}

	/*
	 * TODO: consider delay
	 * k_delayed_work_submit(&notify_work, K_MSEC(1));
	 */
	ret = k_delayed_work_submit(&notify_work, K_NO_WAIT);
	if (ret) {
		SYS_LOG_ERR("Error submittinf delaying queue: %d", ret);
	}
}

static int rndis_init_handle(u8_t *data, u32_t len)
{
	struct rndis_init_cmd *cmd = (void *)data;
	struct rndis_init_cmd_complete *rsp;
	struct net_buf *buf;

	SYS_LOG_DBG("req_id 0x%x", cmd->req_id);

	buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
	if (!buf) {
		SYS_LOG_ERR("Cannot get free buffer");
		return -ENOMEM;
	}

	rsp = net_buf_add(buf, sizeof(*rsp));
	rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
	rsp->type = sys_cpu_to_le32(RNDIS_CMD_INITIALIZE_COMPLETE);
	rsp->len = sys_cpu_to_le32(sizeof(*rsp));
	rsp->req_id = cmd->req_id;

	rsp->major_ver = sys_cpu_to_le32(RNDIS_MAJOR_VERSION);
	rsp->minor_ver = sys_cpu_to_le32(RNDIS_MINOR_VERSION);

	rsp->flags = sys_cpu_to_le32(RNDIS_FLAG_CONNECTIONLESS);
	rsp->medium = sys_cpu_to_le32(RNDIS_MEDIUM_WIRED_ETHERNET);
	rsp->max_packets = sys_cpu_to_le32(1);
	rsp->max_transfer_size = sys_cpu_to_le32(rndis.mtu +
						 sizeof(struct net_eth_hdr) +
						 sizeof(struct
							rndis_payload_packet));

	rsp->pkt_align_factor = sys_cpu_to_le32(0);
	memset(rsp->__reserved, 0, sizeof(rsp->__reserved));

	rndis.state = INITIALIZED;

	rndis_queue_rsp(buf);

	/* Nofity about ready reply */
	rndis_notify_rsp();

	return 0;
}

static int rndis_halt_handle(void)
{
	SYS_LOG_DBG("");

	rndis.state = UNINITIALIZED;

	/* TODO: Stop networking */

	return 0;
}

static uint32_t rndis_query_add_supp_list(struct net_buf *buf)
{
	for (int i = 0; i < ARRAY_SIZE(object_id_supported); i++) {
		net_buf_add_le32(buf, object_id_supported[i]);
	}

	return sizeof(object_id_supported);
}

static int rndis_query_handle(u8_t *data, u32_t len)
{
	struct rndis_query_cmd *cmd = (void *)data;
	struct rndis_query_cmd_complete *rsp;
	struct net_buf *buf;
	u32_t object_id, buf_len = 0;

	buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
	if (!buf) {
		SYS_LOG_ERR("Cannot get free buffer");
		return -ENOMEM;
	}

	object_id = sys_le32_to_cpu(cmd->object_id);

	SYS_LOG_DBG("req_id 0x%x Object ID 0x%x buf_len %u buf_offset %u",
		    sys_le32_to_cpu(cmd->req_id),
		    object_id,
		    sys_le32_to_cpu(cmd->buf_len),
		    sys_le32_to_cpu(cmd->buf_offset));

	rsp = net_buf_add(buf, sizeof(*rsp));
	rsp->type = sys_cpu_to_le32(RNDIS_CMD_QUERY_COMPLETE);
	rsp->req_id = cmd->req_id;

	/* offset is from the beginning of the req_id field */
	rsp->buf_offset = sys_cpu_to_le32(16);

	switch (object_id) {
	case RNDIS_OBJECT_ID_GEN_SUPP_LIST:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_SUPP_LIST");
		rndis_query_add_supp_list(buf);
		break;
	case RNDIS_OBJECT_ID_GEN_PHYSICAL_MEDIUM:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_PHYSICAL_MEDIUM");
		net_buf_add_le32(buf, RNDIS_PHYSICAL_MEDIUM_TYPE_UNSPECIFIED);
		break;
	case RNDIS_OBJECT_ID_GEN_MAX_FRAME_SIZE:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_MAX_FRAME_SIZE");
		net_buf_add_le32(buf, rndis.mtu);
		break;
	case RNDIS_OBJECT_ID_GEN_LINK_SPEED:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_LINK_SPEED");
		if (rndis.media_status == RNDIS_OBJECT_ID_MEDIA_DISCONNECTED) {
			net_buf_add_le32(buf, 0);
		} else {
			net_buf_add_le32(buf, rndis.speed);
		}
		break;
	case RNDIS_OBJECT_ID_GEN_CONN_MEDIA_STATUS:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_CONN_MEDIA_STATUS");
		net_buf_add_le32(buf, rndis.media_status);
		break;
	case RNDIS_OBJECT_ID_GEN_MAX_TOTAL_SIZE:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_MAX_TOTAL_SIZE");
		net_buf_add_le32(buf, RNDIS_GEN_MAX_TOTAL_SIZE);
		break;

		/* Statistics stuff */
#if STATISTICS_ENABLED
	case RNDIS_OBJECT_ID_GEN_TRANSMIT_OK:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_TRANSMIT_OK");
		net_buf_add_le32(rndis.tx_pkts - rndis.tx_err);
		break;
#endif
	case RNDIS_OBJECT_ID_GEN_TRANSMIT_ERROR:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_TRANSMIT_ERROR: %u",
			    rndis.tx_err);
		net_buf_add_le32(buf, rndis.tx_err);
		break;
	case RNDIS_OBJECT_ID_GEN_RECEIVE_ERROR:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_RECEIVE_ERROR: %u",
			    rndis.rx_err);
		net_buf_add_le32(buf, rndis.rx_err);
		break;
	case RNDIS_OBJECT_ID_GEN_RECEIVE_NO_BUF:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_RECEIVE_NO_BUF: %u",
			    rndis.rx_no_buf);
		net_buf_add_le32(buf, rndis.rx_no_buf);
		break;

		/* IEEE 802.3 */
	case RNDIS_OBJECT_ID_802_3_PERMANENT_ADDRESS:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_802_3_PERMANENT_ADDRESS");
		memcpy(net_buf_add(buf, sizeof(rndis.mac)), rndis.mac,
		       sizeof(rndis.mac));
		break;
	case RNDIS_OBJECT_ID_802_3_CURR_ADDRESS:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_802_3_CURR_ADDRESS");
		memcpy(net_buf_add(buf, sizeof(rndis.mac)), rndis.mac,
		       sizeof(rndis.mac));
		break;
	case RNDIS_OBJECT_ID_802_3_MCAST_LIST:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_802_3_MCAST_LIST");
		net_buf_add_le32(buf, 0xE0000000); /* 224.0.0.0 */
		break;
	case RNDIS_OBJECT_ID_802_3_MAX_LIST_SIZE:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_802_3_MAX_LIST_SIZE");
		net_buf_add_le32(buf, 1); /* one address */
		break;

		/* Vendor information */
	case RNDIS_OBJECT_ID_GEN_VENDOR_ID:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_VENDOR_ID");
		net_buf_add_le32(buf, CONFIG_USB_DEVICE_VID);
		break;
	case RNDIS_OBJECT_ID_GEN_VENDOR_DESC:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_VENDOR_DESC");
		memcpy(net_buf_add(buf, sizeof(manufacturer) - 1), manufacturer,
		       sizeof(manufacturer) - 1);
		break;
	case RNDIS_OBJECT_ID_GEN_VENDOR_DRV_VER:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_VENDOR_DRV_VER");
		net_buf_add_le32(buf, drv_version);
		break;
	default:
		SYS_LOG_WRN("Unhandled query for Object ID 0x%x", object_id);
		break;
	}

	buf_len = buf->len - sizeof(*rsp);

	if (buf_len) {
		rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
	} else {
		rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_NOT_SUPP);
	}

	/* Can be zero if object_id not handled / found */
	rsp->buf_len = sys_cpu_to_le32(buf_len);

	rsp->len = sys_cpu_to_le32(buf_len + sizeof(*rsp));

	SYS_LOG_DBG("buf_len %u rsp->len %u buf->len %u",
		    buf_len, rsp->len, buf->len);

	rndis_queue_rsp(buf);

	/* Nofity about ready reply */
	rndis_notify_rsp();

	return 0;
}

static int rndis_set_handle(u8_t *data, u32_t len)
{
	struct rndis_set_cmd *cmd = (void *)data;
	struct rndis_set_cmd_complete *rsp;
	struct net_buf *buf;
	u32_t object_id;
	u8_t *param;

	if (len < sizeof(*cmd)) {
		SYS_LOG_ERR("Packet is shorter then header");
		return -EINVAL;
	}

	/* Parameter starts at offset buf_offset of the req_id field ;) */
	param = (u8_t *)&cmd->req_id + sys_le32_to_cpu(cmd->buf_offset);

	if (len - ((u32_t)param - (u32_t)cmd) !=
	    sys_le32_to_cpu(cmd->buf_len)) {
		SYS_LOG_ERR("Packet parsing error");
		return -EINVAL;
	}

	buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
	if (!buf) {
		SYS_LOG_ERR("Cannot get free buffer");
		return -ENOMEM;
	}

	object_id = sys_le32_to_cpu(cmd->object_id);

	SYS_LOG_DBG("req_id 0x%x Object ID 0x%x buf_len %u buf_offset %u",
		    sys_le32_to_cpu(cmd->req_id), object_id,
		    sys_le32_to_cpu(cmd->buf_len),
		    sys_le32_to_cpu(cmd->buf_offset));

	rsp = net_buf_add(buf, sizeof(*rsp));
	rsp->type = sys_cpu_to_le32(RNDIS_CMD_SET_COMPLETE);
	rsp->len = sys_cpu_to_le32(sizeof(*rsp));
	rsp->req_id = cmd->req_id; /* same endianness */

	switch (object_id) {
	case RNDIS_OBJECT_ID_GEN_PKT_FILTER:
		if (sys_le32_to_cpu(cmd->buf_len) < sizeof(rndis.net_filter)) {
			SYS_LOG_ERR("Packet is too small");
			rsp->status = RNDIS_CMD_STATUS_INVALID_DATA;
			break;
		}

		rndis.net_filter = sys_get_le32(param);
		SYS_LOG_DBG("RNDIS_OBJECT_ID_GEN_PKT_FILTER 0x%x",
			    rndis.net_filter);
		/* TODO: Start / Stop networking here */
		rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
		break;
	case RNDIS_OBJECT_ID_802_3_MCAST_LIST:
		SYS_LOG_DBG("RNDIS_OBJECT_ID_802_3_MCAST_LIST");
		/* ignore for now */
		rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
		break;
	default:
		SYS_LOG_ERR("Unhandled object_id 0x%x", object_id);
		rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_NOT_SUPP);
		break;
	}

	rndis_queue_rsp(buf);

	/* Nofity about ready reply */
	rndis_notify_rsp();

	return 0;
}

static int rndis_reset_handle(u8_t *data, u32_t len)
{
	struct rndis_reset_cmd_complete *rsp;
	struct net_buf *buf;

	buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
	if (!buf) {
		SYS_LOG_ERR("Cannot get free buffer");
		return -ENOMEM;
	}

	SYS_LOG_DBG("");

	rsp = net_buf_add(buf, sizeof(*rsp));
	rsp->type = sys_cpu_to_le32(RNDIS_CMD_RESET_COMPLETE);
	rsp->len = sys_cpu_to_le32(sizeof(*rsp));
	rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);
	rsp->addr_reset = sys_cpu_to_le32(1);

	rndis_queue_rsp(buf);

	/* Nofity about ready reply */
	rndis_notify_rsp();

	return 0;
}

static int rndis_keepalive_handle(u8_t *data, u32_t len)
{
	struct rndis_keepalive_cmd *cmd = (void *)data;
	struct rndis_keepalive_cmd_complete *rsp;
	struct net_buf *buf;

	buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
	if (!buf) {
		SYS_LOG_ERR("Cannot get free buffer");
		return -ENOMEM;
	}

	SYS_LOG_DBG("");

	rsp = net_buf_add(buf, sizeof(*rsp));
	rsp->type = sys_cpu_to_le32(RNDIS_CMD_KEEPALIVE_COMPLETE);
	rsp->len = sys_cpu_to_le32(sizeof(*rsp));
	rsp->req_id = cmd->req_id; /* same order */
	rsp->status = sys_cpu_to_le32(RNDIS_CMD_STATUS_SUCCESS);

	rndis_queue_rsp(buf);

	/* Nofity about ready reply */
	rndis_notify_rsp();

	return 0;
}

static int queue_encapsulated_cmd(u8_t *data, u32_t len)
{
	struct net_buf *buf;

	buf = net_buf_alloc(&rndis_cmd_pool, K_NO_WAIT);
	if (!buf) {
		SYS_LOG_ERR("Cannot get free buffer");
		return -ENOMEM;
	}

	memcpy(net_buf_add(buf, len), data, len);

	net_buf_put(&rndis_cmd_queue, buf);

	SYS_LOG_DBG("queued buf %p", buf);

	return 0;
}

static int handle_encapsulated_cmd(u8_t *data, u32_t len)
{
	struct tlv *msg = (void *)data;

	net_hexdump("CMD >", data, len);

	if (len != msg->len) {
		SYS_LOG_WRN("Total len is different then command len %u %u",
			    len, msg->len);
		/* TODO: need actions? */
	}

	SYS_LOG_DBG("RNDIS type 0x%x len %u total len %u",
		    msg->type, msg->len, len);

	switch (msg->type) {
	case RNDIS_CMD_INITIALIZE:
		return rndis_init_handle(data, len);
	case RNDIS_CMD_HALT:
		return rndis_halt_handle();
	case RNDIS_CMD_QUERY:
		return rndis_query_handle(data, len);
	case RNDIS_CMD_SET:
		return rndis_set_handle(data, len);
	case RNDIS_CMD_RESET:
		return rndis_reset_handle(data, len);
	case RNDIS_CMD_KEEPALIVE:
		return rndis_keepalive_handle(data, len);
	default:
		SYS_LOG_ERR("Message 0x%x unhandled", msg->type);
		return -ENOTSUP;
	}

	return 0;
}

#if SEND_MEDIA_STATUS
static int rndis_send_media_status(u32_t media_status)
{
	struct rndis_media_status_indicate *ind;
	struct net_buf *buf;

	SYS_LOG_DBG("status %u", media_status);

	buf = net_buf_alloc(&rndis_tx_pool, K_NO_WAIT);
	if (!buf) {
		SYS_LOG_ERR("Cannot get free buffer");
		return -ENOMEM;
	}

	ind = net_buf_add(buf, sizeof(*ind));
	ind->type = sys_cpu_to_le32(RNDIS_CMD_INDICATE);
	ind->len = sys_cpu_to_le32(sizeof(*ind));

	if (media_status) {
		ind->status = sys_cpu_to_le32(RNDIS_STATUS_CONNECT_MEDIA);
	} else {
		ind->status = sys_cpu_to_le32(RNDIS_STATUS_DISCONNECT_MEDIA);
	}

	ind->buf_len = 0;
	ind->buf_offset = 0;

	rndis_queue_rsp(buf);

	/* Nofity about ready reply */
	rndis_notify_rsp();

	return 0;
}
#endif /* SEND_MEDIA_STATUS */

static int handle_encapsulated_rsp(u8_t **data, u32_t *len)
{
	struct net_buf *buf;

	SYS_LOG_DBG("");

	buf = net_buf_get(&rndis_tx_queue, K_NO_WAIT);
	if (!buf) {
		SYS_LOG_ERR("Error getting response buffer");
		*len = 0;
		return -ENODATA;
	}

	net_hexdump("RSP <", buf->data, buf->len);

	memcpy(*data, buf->data, buf->len);
	*len = buf->len;

	net_buf_unref(buf);

	return 0;
}

static int rndis_class_handler(struct usb_setup_packet *setup, s32_t *len,
			       u8_t **data)
{
	SYS_LOG_DBG("");

	if (setup->bRequest == CDC_SEND_ENC_CMD &&
	    REQTYPE_GET_DIR(setup->bmRequestType) == REQTYPE_DIR_TO_DEVICE) {
		/*
		 * Instead of handling here, queue
		 * handle_encapsulated_cmd(*data, *len);
		 */
		queue_encapsulated_cmd(*data, *len);
	} else if (setup->bRequest == CDC_GET_ENC_RSP &&
		   REQTYPE_GET_DIR(setup->bmRequestType) ==
		   REQTYPE_DIR_TO_HOST) {
		handle_encapsulated_rsp(data, len);
	} else {
		*len = 0; /* FIXME! */
		SYS_LOG_WRN("Unknown USB packet req 0x%x type 0x%x",
			    setup->bRequest, setup->bmRequestType);
	}

	return 0;
}

static void cmd_thread(void)
{
	SYS_LOG_INF("Command thread started");

	while (true) {
		struct net_buf *buf;

		buf = net_buf_get(&rndis_cmd_queue, K_FOREVER);

		SYS_LOG_DBG("got buf %p", buf);

		handle_encapsulated_cmd(buf->data, buf->len);

		net_buf_unref(buf);

		k_yield();
	}
}

/*
 * RNDIS Send functions
 */

static void rndis_hdr_add(u8_t *buf, u32_t len)
{
	struct rndis_payload_packet *hdr = (void *)buf;
	u32_t offset = offsetof(struct rndis_payload_packet, payload_offset);

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

	hdr->type = sys_cpu_to_le32(RNDIS_DATA_PACKET);
	hdr->len = sys_cpu_to_le32(len + sizeof(*hdr));
	hdr->payload_offset = sys_cpu_to_le32(sizeof(*hdr) - offset);
	hdr->payload_len = sys_cpu_to_le32(len);

	SYS_LOG_DBG("type %u len %u payload offset %u payload len %u",
		    hdr->type, hdr->len, hdr->payload_offset, hdr->payload_len);
}

/*
 * The idea here is to use one buffer of size endpoint MPS (64 bytes)
 * for sending pkt without linearlizing first since we would need Ethernet
 * packet frame as a buffer up to 1518 bytes and it would require two
 * iterations.
 */
static int append_bytes(u8_t *out_buf, u16_t buf_len, u8_t *data,
			u16_t len, u16_t remaining)
{
	int ret;

	do {
		u16_t count = min(len, remaining);
#if VERBOSE_DEBUG
		SYS_LOG_DBG("len %u remaining %u count %u", len, remaining,
			    count);
#endif

		memcpy(out_buf + (buf_len - remaining), data, count);

		data += count;
		remaining -= count;
		len -= count;

		/* Buffer filled */
		if (remaining == 0) {
#if VERBOSE_DEBUG
			net_hexdump("fragment", out_buf, buf_len);
#endif

			ret = try_write(CONFIG_RNDIS_IN_EP_ADDR, out_buf,
					buf_len);
			if (ret) {
				SYS_LOG_ERR("Error sending data");
				return ret;
			}

			/* Consumed full buffer */
			if (len == 0) {
				return buf_len;
			}

			remaining = buf_len;
		}
	} while (len);
#if VERBOSE_DEBUG
	net_hexdump("fragment", out_buf, buf_len - remaining);
#endif
	return remaining;
}

static int rndis_send(struct net_pkt *pkt)
{
	u8_t buf[CONFIG_RNDIS_BULK_EP_MPS];
	int remaining = sizeof(buf);
	struct net_buf *frag;

	SYS_LOG_DBG("send pkt %p len %u", pkt, net_pkt_get_len(pkt));

	if (rndis.media_status == RNDIS_OBJECT_ID_MEDIA_DISCONNECTED) {
		SYS_LOG_DBG("Media disconnected, drop pkt %p", pkt);
		return -EPIPE;
	}

	net_hexdump_frags("<", pkt);

	if (!pkt->frags) {
		return -ENODATA;
	}

	rndis_hdr_add(buf, net_pkt_get_len(pkt) + net_pkt_ll_reserve(pkt));

	remaining -= sizeof(struct rndis_payload_packet);

	remaining = append_bytes(buf, sizeof(buf), net_pkt_ll(pkt),
				 net_pkt_ll_reserve(pkt) + pkt->frags->len,
				 remaining);
	if (remaining < 0) {
		return remaining;
	}

	for (frag = pkt->frags->frags; frag; frag = frag->frags) {
		SYS_LOG_DBG("Fragment %p len %u remaining %u",
			    frag, frag->len, remaining);
		remaining = append_bytes(buf, sizeof(buf), frag->data,
					 frag->len, remaining);
		if (remaining < 0) {
			return remaining;
		}
	}

	if (remaining > 0 && remaining < sizeof(buf)) {
		return try_write(CONFIG_RNDIS_IN_EP_ADDR, buf,
				 sizeof(buf) - remaining);
	} else {
		rndis_send_zero_frame();
	}

	return 0;
}

static int rndis_init(void)
{
	SYS_LOG_DBG("");

	/* Transmit queue init */
	k_fifo_init(&rndis_tx_queue);
	/* Command queue init */
	k_fifo_init(&rndis_cmd_queue);

	k_delayed_work_init(&notify_work, rndis_notify);

	k_thread_create(&cmd_thread_data, cmd_stack,
			K_THREAD_STACK_SIZEOF(cmd_stack),
			(k_thread_entry_t)cmd_thread,
			NULL, NULL, NULL, K_PRIO_COOP(8), 0, K_NO_WAIT);

	return 0;
}

static int rndis_connect_media(bool status)
{
	if (status) {
		rndis.media_status = RNDIS_OBJECT_ID_MEDIA_CONNECTED;
	} else {
		rndis.media_status = RNDIS_OBJECT_ID_MEDIA_DISCONNECTED;
	}

#if SEND_MEDIA_STATUS
	return rndis_send_media_status(status);
#else
	return 0;
#endif
}

static struct usb_ep_cfg_data rndis_ep_data[] = {
	{
		.ep_cb = rndis_int_in,
		.ep_addr = CONFIG_RNDIS_INT_EP_ADDR
	},
	{
		.ep_cb = rndis_bulk_out,
		.ep_addr = CONFIG_RNDIS_OUT_EP_ADDR
	},
	{
		.ep_cb = rndis_bulk_in,
		.ep_addr = CONFIG_RNDIS_IN_EP_ADDR
	},
};

struct netusb_function rndis_function = {
	.init = rndis_init,
	.connect_media = rndis_connect_media,
	.class_handler = rndis_class_handler,
	.send_pkt = rndis_send,
	.num_ep = ARRAY_SIZE(rndis_ep_data),
	.ep = rndis_ep_data,
};