refactor(network): Refactor usbip and tcp process code

In previous versions, parsing of the usbip protocol was implemented
via a global state machine and shared with elaphureLink.

The refactored code removes these global state machines and handles them
with the corresponding protocol header commands.

components/elaphureLink/elaphureLink_protocol.c

@@ -1,11 +1,13 @@
 #include "components/elaphureLink/elaphureLink_protocol.h"
 
+#include "main/DAP_handle.h"
 
 #include "lwip/err.h"
 #include "lwip/sockets.h"
 #include "lwip/sys.h"
 #include <lwip/netdb.h>
 
+extern int kRestartDAPHandle;
 extern int kSock;
 extern int usbip_network_send(int s, const void *dataptr, size_t size, int flags);
 
@@ -25,7 +27,6 @@ void el_process_buffer_malloc() {
     el_process_buffer = malloc(1500);
 }
 
-
 void el_process_buffer_free() {
     if (el_process_buffer != NULL) {
         free(el_process_buffer);
@@ -33,7 +34,6 @@ void el_process_buffer_free() {
     }
 }
 
-
 int el_handshake_process(int fd, void *buffer, size_t len) {
     if (len != sizeof(el_request_handshake)) {
         return -1;
@@ -59,10 +59,42 @@ int el_handshake_process(int fd, void *buffer, size_t len) {
     return 0;
 }
 
-
 void el_dap_data_process(void* buffer, size_t len) {
     int res = DAP_ExecuteCommand(buffer, (uint8_t *)el_process_buffer);
     res &= 0xFFFF;
 
     usbip_network_send(kSock, el_process_buffer, res, 0);
 }
+
+int el_dap_work(uint8_t* base, size_t len)
+{
+    uint8_t *data;
+    int sz, ret;
+
+    // read command code and protocol version
+    data = base + 4;
+    sz = 8;
+    do {
+        ret = recv(kSock, data, sz, 0);
+        if (ret <= 0)
+            return ret;
+        sz -= ret;
+        data += ret;
+    } while (sz > 0);
+
+    ret = el_handshake_process(kSock, base, 12);
+    if (ret)
+        return ret;
+
+    kRestartDAPHandle = DELETE_HANDLE;
+    el_process_buffer_malloc();
+    // data process
+    while(1) {
+        ret = recv(kSock, base, len, 0);
+        if (ret <= 0)
+            return ret;
+        el_dap_data_process(base, ret);
+    }
+
+    return 0;
+}

components/elaphureLink/elaphureLink_protocol.h

@@ -47,6 +47,8 @@ int el_handshake_process(int fd, void* buffer, size_t len);
 void el_dap_data_process(void* buffer, size_t len);
 
 
+int el_dap_work(uint8_t* base, size_t len);
+
 void el_process_buffer_malloc();
 void el_process_buffer_free();
 

main/CMakeLists.txt

@@ -1,6 +1,6 @@
 set(COMPONENT_ADD_INCLUDEDIRS "${PROJECT_PATH}")
 set(COMPONENT_SRCS
     main.c timer.c tcp_server.c usbip_server.c DAP_handle.c
-    kcp_server.c tcp_netconn.c uart_bridge.c wifi_handle.c)
+    uart_bridge.c wifi_handle.c)
 
 register_component()

main/DAP_handle.c

@@ -111,7 +111,7 @@ void handle_dap_data_request(usbip_stage2_header *header, uint32_t length)
     // Point to the beginning of the URB packet
 
 #if (USE_WINUSB == 1)
-    send_stage2_submit(header, 0, 0);
+    send_stage2_submit_data_fast(header, NULL, 0);
 
     // always send constant size buf -> cuz we don't care about the IN packet size
     // and to unify the style, we set aside the length of the section
@@ -119,7 +119,7 @@ void handle_dap_data_request(usbip_stage2_header *header, uint32_t length)
     xTaskNotifyGive(kDAPTaskHandle);
 
 #else
-    send_stage2_submit(header, 0, 0);
+    send_stage2_submit_data_fast(header, NULL, 0);
 
     xRingbufferSend(dap_dataIN_handle, data_in, DAP_HANDLE_SIZE, portMAX_DELAY);
     xTaskNotifyGive(kDAPTaskHandle);
@@ -131,22 +131,6 @@ void handle_dap_data_request(usbip_stage2_header *header, uint32_t length)
     // send_stage2_submit(header, 0, 0);
 }
 
-void handle_dap_data_response(usbip_stage2_header *header)
-{
-    return;
-    // int resLength = dap_respond & 0xFFFF;
-    // if (resLength)
-    // {
-
-    //     send_stage2_submit_data(header, 0, (void *)DAPDataProcessed.buf, resLength);
-    //     dap_respond = 0;
-    // }
-    // else
-    // {
-    //     send_stage2_submit(header, 0, 0);
-    // }
-}
-
 void handle_swo_trace_response(usbip_stage2_header *header)
 {
 #if (SWO_FUNCTION_ENABLE == 1)
@@ -261,54 +245,43 @@ void DAP_Thread(void *argument)
     }
 }
 
-int fast_reply(uint8_t *buf, uint32_t length)
+int fast_reply(uint8_t *buf, uint32_t length, int dap_req_num)
 {
     usbip_stage2_header *buf_header = (usbip_stage2_header *)buf;
-    if (length == 48 &&
+
-        buf_header->base.command == PP_HTONL(USBIP_STAGE2_REQ_SUBMIT) &&
+    if (dap_req_num > 0) {
-        buf_header->base.direction == PP_HTONL(USBIP_DIR_IN) &&
+        DapPacket_t *item;
-        buf_header->base.ep == PP_HTONL(1))
+        size_t packetSize = 0;
-    {
+        item = (DapPacket_t *)xRingbufferReceiveUpTo(dap_dataOUT_handle, &packetSize,
-        if (dap_respond > 0)
+                                                     portMAX_DELAY, DAP_HANDLE_SIZE);
-        {
+        if (packetSize == DAP_HANDLE_SIZE) {
-            DapPacket_t *item;
-            size_t packetSize = 0;
-            item = (DapPacket_t *)xRingbufferReceiveUpTo(dap_dataOUT_handle, &packetSize,
-                                                         pdMS_TO_TICKS(10), DAP_HANDLE_SIZE);
-            if (packetSize == DAP_HANDLE_SIZE)
-            {
 #if (USE_WINUSB == 1)
-                send_stage2_submit_data_fast((usbip_stage2_header *)buf, item->buf, item->length);
+            send_stage2_submit_data_fast((usbip_stage2_header *)buf, item->buf, item->length);
 #else
-                send_stage2_submit_data_fast((usbip_stage2_header *)buf, item->buf, DAP_HANDLE_SIZE);
+            send_stage2_submit_data_fast((usbip_stage2_header *)buf, item->buf, DAP_HANDLE_SIZE);
 #endif
 
-                if (xSemaphoreTake(data_response_mux, portMAX_DELAY) == pdTRUE)
+            if (xSemaphoreTake(data_response_mux, portMAX_DELAY) == pdTRUE) {
-                {
+                --dap_respond;
-                    --dap_respond;
+                xSemaphoreGive(data_response_mux);
-                    xSemaphoreGive(data_response_mux);
+            }
-                }
 
-                vRingbufferReturnItem(dap_dataOUT_handle, (void *)item);
+            vRingbufferReturnItem(dap_dataOUT_handle, (void *)item);
-                return 1;
-            }
-            else if (packetSize > 0)
-            {
-                os_printf("Wrong data out packet size:%d!\r\n", packetSize);
-            }
-            ////TODO: fast reply
-        }
-        else
-        {
-            buf_header->base.command = PP_HTONL(USBIP_STAGE2_RSP_SUBMIT);
-            buf_header->base.direction = PP_HTONL(USBIP_DIR_OUT);
-            buf_header->u.ret_submit.status = 0;
-            buf_header->u.ret_submit.data_length = 0;
-            buf_header->u.ret_submit.error_count = 0;
-            usbip_network_send(kSock, buf, 48, 0);
             return 1;
+        } else if (packetSize > 0) {
+            os_printf("Wrong data out packet size:%d!\r\n", packetSize);
         }
+        ////TODO: fast reply
+    } else {
+        buf_header->base.command = PP_HTONL(USBIP_STAGE2_RSP_SUBMIT);
+        buf_header->base.direction = PP_HTONL(USBIP_DIR_OUT);
+        buf_header->u.ret_submit.status = 0;
+        buf_header->u.ret_submit.data_length = 0;
+        buf_header->u.ret_submit.error_count = 0;
+        usbip_network_send(kSock, buf, 48, 0);
+        return 1;
     }
+
     return 0;
 }
 

main/DAP_handle.h

@@ -11,10 +11,9 @@ enum reset_handle_t
 };
 
 void handle_dap_data_request(usbip_stage2_header *header, uint32_t length);
-void handle_dap_data_response(usbip_stage2_header *header);
 void handle_swo_trace_response(usbip_stage2_header *header);
 void handle_dap_unlink();
 
-int fast_reply(uint8_t *buf, uint32_t length);
+int fast_reply(uint8_t *buf, uint32_t length, int dap_req_num);
 
 #endif

main/tcp_server.c

@@ -34,15 +34,18 @@
 extern TaskHandle_t kDAPTaskHandle;
 extern int kRestartDAPHandle;
 
-uint8_t kState = ACCEPTING;
 int kSock = -1;
 
 void tcp_server_task(void *pvParameters)
 {
     uint8_t tcp_rx_buffer[1500];
     char addr_str[128];
+    enum usbip_server_state_t usbip_state = WAIT_DEVLIST;
+    uint8_t *data;
     int addr_family;
     int ip_protocol;
+    int header;
+    int ret, sz;
 
     int on = 1;
     while (1)
@@ -111,75 +114,39 @@ void tcp_server_task(void *pvParameters)
             setsockopt(kSock, IPPROTO_TCP, TCP_NODELAY, (void *)&on, sizeof(on));
             os_printf("Socket accepted\r\n");
 
-            while (1)
+            // Read header
-            {
+            sz = 4;
-                int len = recv(kSock, tcp_rx_buffer, sizeof(tcp_rx_buffer), 0);
+            data = &tcp_rx_buffer[0];
-                // Error occured during receiving
+            do {
-                if (len < 0)
+                ret = recv(kSock, data, sz, 0);
-                {
+                if (ret <= 0)
-                    os_printf("recv failed: errno %d\r\n", errno);
+                    goto cleanup;
-                    break;
+                sz -= ret;
-                }
+                data += ret;
-                // Connection closed
+            } while (sz > 0);
-                else if (len == 0)
+
-                {
+            header = *((int *)(tcp_rx_buffer));
-                    os_printf("Connection closed\r\n");
+            header = ntohl(header);
-                    break;
+
-                }
+            if (header == EL_LINK_IDENTIFIER) {
-                // Data received
+                el_dap_work(tcp_rx_buffer, sizeof(tcp_rx_buffer));
+            } else if ((header & 0xFFFF) == 0x8003 ||
+                       (header & 0xFFFF) == 0x8005) { // usbip OP_REQ_DEVLIST/OP_REQ_IMPORT
+                if ((header & 0xFFFF) == 0x8005)
+                    usbip_state = WAIT_DEVLIST;
                 else
-                {
+                    usbip_state = WAIT_IMPORT;
-                    // #ifdef CONFIG_EXAMPLE_IPV6
+                usbip_worker(tcp_rx_buffer, sizeof(tcp_rx_buffer), &usbip_state);
-                    //                     // Get the sender's ip address as string
+            } else {
-                    //                     if (sourceAddr.sin6_family == PF_INET)
+                os_printf("Unknown protocol\n");
-                    //                     {
-                    //                         inet_ntoa_r(((struct sockaddr_in *)&sourceAddr)->sin_addr.s_addr, addr_str, sizeof(addr_str) - 1);
-                    //                     }
-                    //                     else if (sourceAddr.sin6_family == PF_INET6)
-                    //                     {
-                    //                         inet6_ntoa_r(sourceAddr.sin6_addr, addr_str, sizeof(addr_str) - 1);
-                    //                     }
-                    // #else
-                    //                     inet_ntoa_r(((struct sockaddr_in *)&sourceAddr)->sin_addr.s_addr, addr_str, sizeof(addr_str) - 1);
-                    // #endif
-
-                    switch (kState)
-                    {
-                    case ACCEPTING:
-                        kState = ATTACHING;
-
-                    case ATTACHING:
-                        // elaphureLink handshake
-                        if (el_handshake_process(kSock, tcp_rx_buffer, len) == 0) {
-                            // handshake successed
-                            kState = EL_DATA_PHASE;
-                            kRestartDAPHandle = DELETE_HANDLE;
-                            el_process_buffer_malloc();
-                            break;
-                        }
-
-                        attach(tcp_rx_buffer, len);
-                        break;
-
-                    case EMULATING:
-                        emulate(tcp_rx_buffer, len);
-                        break;
-                    case EL_DATA_PHASE:
-                        el_dap_data_process(tcp_rx_buffer, len);
-                        break;
-                    default:
-                        os_printf("unkonw kstate!\r\n");
-                    }
-                }
             }
-            // kState = ACCEPTING;
+
+cleanup:
             if (kSock != -1)
             {
                 os_printf("Shutting down socket and restarting...\r\n");
                 //shutdown(kSock, 0);
                 close(kSock);
-                if (kState == EMULATING || kState == EL_DATA_PHASE)
-                    kState = ACCEPTING;
 
                 // Restart DAP Handle
                 el_process_buffer_free();

main/usbip_server.c

@@ -28,8 +28,6 @@ static void send_interface_info();
 // emulate helper function
 static void pack(void *data, int size);
 static void unpack(void *data, int size);
-static int handle_submit(usbip_stage2_header *header, uint32_t length);
-static int read_stage2_command(usbip_stage2_header *header, uint32_t length);
 
 static void handle_unlink(usbip_stage2_header *header);
 // unlink helper function
@@ -45,7 +43,7 @@ int usbip_network_send(int s, const void *dataptr, size_t size, int flags) {
 #endif
 }
 
-int attach(uint8_t *buffer, uint32_t length)
+static int attach(uint8_t *buffer, uint32_t length)
 {
     int command = read_stage1_command(buffer, length);
     if (command < 0)
@@ -102,8 +100,6 @@ static void handle_device_attach(uint8_t *buffer, uint32_t length)
     send_stage1_header(USBIP_STAGE1_CMD_DEVICE_ATTACH, 0);
 
     send_device_info();
-
-    kState = EMULATING;
 }
 
 static void send_stage1_header(uint16_t command, uint32_t status)
@@ -181,57 +177,126 @@ static void send_interface_info()
     usbip_network_send(kSock, (uint8_t *)&interface, sizeof(usbip_stage1_usb_interface), 0);
 }
 
-///////////////////////////////////////////////////////////////////////////
+static int usbip_urb_process(uint8_t *base, uint32_t length)
-///////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////
-///////////////////////////////////////////////////////////////////////////
-
-int emulate(uint8_t *buffer, uint32_t length)
 {
+    usbip_stage2_header *header = (usbip_stage2_header *)base;
+    uint8_t *data;
+    uint32_t command, dir, ep;
+    uint32_t unlink_count = 0;
+    bool may_has_data;
+    int sz, ret;
+    int dap_req_num = 0;
 
-    if(fast_reply(buffer, length))
+    while (1) {
-    {
+        // header
-        return 0;
+        data = base;
+        sz = 48; // for USBIP_CMD_SUBMIT/USBIP_CMD_UNLINK
+        do {
+            ret = recv(kSock, data, sz, 0);
+            if (ret <= 0)
+                goto out;
+            sz -= ret;
+            data += ret;
+        } while (sz > 0);
+
+        command = ntohl(header->base.command);
+        dir = ntohl(header->base.direction);
+        ep = ntohl(header->base.ep);
+        may_has_data = (command == USBIP_STAGE2_REQ_SUBMIT && dir == USBIP_DIR_OUT);
+        sz = may_has_data ? ntohl(header->u.cmd_submit.data_length) : 0;
+
+        while (sz) {
+                ret = recv(kSock, data, sz, 0);
+                if (ret <= 0)
+                    goto out;
+                sz -= ret;
+                data += ret;
+        }
+
+        if (likely(command == USBIP_STAGE2_REQ_SUBMIT)) {
+            if (likely(ep == 1 && dir == USBIP_DIR_IN)) {
+                fast_reply(base, sizeof(usbip_stage2_header), dap_req_num);
+                if (dap_req_num > 0)
+                    dap_req_num--;
+            } else if (likely(ep == 1 && dir == USBIP_DIR_OUT)) {
+                dap_req_num++;
+                handle_dap_data_request(header, length);
+            } else if (ep == 0) {
+                unpack(base, sizeof(usbip_stage2_header));
+                handleUSBControlRequest(header);
+            } else {
+                // ep3 reserved for SWO
+                os_printf("ep reserved:%d\r\n", ep);
+                send_stage2_submit(header, 0, 0);
+            }
+        } else if (command == USBIP_STAGE2_REQ_UNLINK) {
+            if (unlink_count == 0 || unlink_count % 100 == 0)
+                os_printf("unlink\r\n");
+            unlink_count++;
+            unpack(base, sizeof(usbip_stage2_header));
+            handle_unlink(header);
+        } else {
+            os_printf("emulate unknown command:%d\r\n", command);
+            return -1;
+        }
     }
 
-    int command = read_stage2_command((usbip_stage2_header *)buffer, length);
+out:
-    if (command < 0)
+    if (ret < 0)
-    {
+        os_printf("recv failed: errno %d\r\n", errno);
-        return -1;
+    return ret;
-    }
-
-    switch (command)
-    {
-    case USBIP_STAGE2_REQ_SUBMIT:
-        handle_submit((usbip_stage2_header *)buffer , length);
-        break;
-
-    case USBIP_STAGE2_REQ_UNLINK:
-        handle_unlink((usbip_stage2_header *)buffer);
-        break;
-
-    default:
-        os_printf("emulate unknown command:%d\r\n", command);
-        //handle_submit((usbip_stage2_header *)buffer, length);
-        return -1;
-    }
-    return 0;
 }
 
-static int read_stage2_command(usbip_stage2_header *header, uint32_t length)
+int usbip_worker(uint8_t *base, uint32_t length, enum usbip_server_state_t *state)
 {
-    if (length < sizeof(usbip_stage2_header))
+    uint8_t *data;
-    {
+    int pre_read_sz = 4;
-        return -1;
+    int sz, ret;
+
+    // OP_REQ_DEVLIST status field
+    if (*state == WAIT_DEVLIST) {
+        data = base + 4;
+        sz = 8 - pre_read_sz;
+        do {
+            ret = recv(kSock, data, sz, 0);
+            if (ret <= 0)
+                return ret;
+            sz -= ret;
+            data += ret;
+        } while (sz > 0);
+
+        ret = attach(base, 8);
+        if (ret)
+            return ret;
+
+        pre_read_sz = 0;
     }
 
-    //client.readBytes((uint8_t *)&header, sizeof(usbip_stage2_header));
+    *state = WAIT_IMPORT;
-    unpack((uint32_t *)header, sizeof(usbip_stage2_header));
+    // OP_REQ_IMPORT
-    return header->base.command;
+    data = base + pre_read_sz;
+    sz = 40 - pre_read_sz;
+    do {
+        ret = recv(kSock, data, sz, 0);
+        if (ret <= 0)
+            return ret;
+        sz -= ret;
+        data += ret;
+    } while (sz > 0);
+
+    ret = attach(base, 40);
+    if (ret)
+        return ret;
+
+    // URB process
+    *state = WAIT_URB;
+    ret = usbip_urb_process(base, length);
+    if (ret) {
+        *state = WAIT_DEVLIST;
+        return ret;
+    }
+
+    return 0;
 }
 
 /**
@@ -281,65 +346,6 @@ static void unpack(void *data, int size)
     }
 }
 
-/**
- * @brief USB transaction processing
- *
- */
-static int handle_submit(usbip_stage2_header *header, uint32_t length)
-{
-    switch (header->base.ep)
-    {
-    // control endpoint(endpoint 0)
-    case 0x00:
-        //// TODO: judge usb setup 8 byte?
-        handleUSBControlRequest(header);
-        break;
-
-    // endpoint 1 data receicve and response
-    case 0x01:
-        if (header->base.direction == 0)
-        {
-            //os_printf("EP 01 DATA FROM HOST");
-            handle_dap_data_request(header ,length);
-        }
-        else
-        {
-            // os_printf("EP 01 DATA TO HOST\r\n");
-            handle_dap_data_response(header);
-        }
-        break;
-    // endpoint 2 for SWO trace
-    case 0x02:
-        if (header->base.direction == 0)
-        {
-            // os_printf("EP 02 DATA FROM HOST");
-            send_stage2_submit(header, 0, 0);
-        }
-        else
-        {
-            // os_printf("EP 02 DATA TO HOST");
-            handle_swo_trace_response(header);
-        }
-        break;
-    // request to save data to device
-    case 0x81:
-        if (header->base.direction == 0)
-        {
-            os_printf("*** WARN! EP 81 DATA TX");
-        }
-        else
-        {
-            os_printf("*** WARN! EP 81 DATA RX");
-        }
-        return -1;
-
-    default:
-        os_printf("*** WARN ! UNKNOWN ENDPOINT: %d\r\n", (int)header->base.ep);
-        return -1;
-    }
-    return 0;
-}
-
 void send_stage2_submit(usbip_stage2_header *req_header, int32_t status, int32_t data_length)
 {
 
@@ -376,16 +382,15 @@ void send_stage2_submit_data_fast(usbip_stage2_header *req_header, const void *c
     memset(&(req_header->u.ret_submit), 0, sizeof(usbip_stage2_header_ret_submit));
     req_header->u.ret_submit.data_length = htonl(data_length);
 
-
     // payload
-    memcpy(&send_buf[sizeof(usbip_stage2_header)], data, data_length);
+    if (data)
+        memcpy(&send_buf[sizeof(usbip_stage2_header)], data, data_length);
     usbip_network_send(kSock, send_buf, sizeof(usbip_stage2_header) + data_length, 0);
 }
 
 
 static void handle_unlink(usbip_stage2_header *header)
 {
-    os_printf("s2 handling cmd unlink...\r\n");
     handle_dap_unlink();
     send_stage2_unlink(header);
 }
@@ -407,4 +412,4 @@ static void send_stage2_unlink(usbip_stage2_header *req_header)
     pack(req_header, sizeof(usbip_stage2_header));
 
     usbip_network_send(kSock, req_header, sizeof(usbip_stage2_header), 0);
-}
+}

main/usbip_server.h

@@ -5,18 +5,16 @@
 
 #include "components/USBIP/usbip_defs.h"
 
-enum state_t
+enum usbip_server_state_t
 {
-    ACCEPTING,
+    WAIT_DEVLIST = 0,
-    ATTACHING,
+    WAIT_IMPORT,
-    EMULATING,
+    WAIT_URB,
-    EL_DATA_PHASE
 };
-extern uint8_t kState;
+
 extern int kSock;
 
-int attach(uint8_t *buffer, uint32_t length);
+int usbip_worker(uint8_t *base, uint32_t length, enum usbip_server_state_t *state);
-int emulate(uint8_t *buffer, uint32_t length);
 void send_stage2_submit_data(usbip_stage2_header *req_header, int32_t status, const void * const data, int32_t data_length);
 void send_stage2_submit(usbip_stage2_header *req_header, int32_t status, int32_t data_length);
 void send_stage2_submit_data_fast(usbip_stage2_header *req_header, const void *const data, int32_t data_length);