| /* |
| * 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(¬ify_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(¬ify_work, K_MSEC(1)); |
| */ |
| ret = k_delayed_work_submit(¬ify_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(¬ify_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, |
| }; |