From 16f8b53a76acccc99b8894693d274a3932637518 Mon Sep 17 00:00:00 2001 From: windowsair Date: Fri, 16 Feb 2024 18:46:50 +0800 Subject: [PATCH] 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. --- .../elaphureLink/elaphureLink_protocol.c | 38 ++- .../elaphureLink/elaphureLink_protocol.h | 2 + main/CMakeLists.txt | 2 +- main/DAP_handle.c | 85 +++---- main/DAP_handle.h | 3 +- main/tcp_server.c | 93 +++----- main/usbip_server.c | 225 +++++++++--------- main/usbip_server.h | 14 +- 8 files changed, 219 insertions(+), 243 deletions(-) diff --git a/components/elaphureLink/elaphureLink_protocol.c b/components/elaphureLink/elaphureLink_protocol.c index c86db73..ab8ec4b 100644 --- a/components/elaphureLink/elaphureLink_protocol.c +++ b/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 +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; +} diff --git a/components/elaphureLink/elaphureLink_protocol.h b/components/elaphureLink/elaphureLink_protocol.h index 751c024..2ba65e6 100644 --- a/components/elaphureLink/elaphureLink_protocol.h +++ b/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(); diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index 3572cc8..9a7536b 100644 --- a/main/CMakeLists.txt +++ b/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() diff --git a/main/DAP_handle.c b/main/DAP_handle.c index 95f7731..4b26058 100644 --- a/main/DAP_handle.c +++ b/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) && - buf_header->base.direction == PP_HTONL(USBIP_DIR_IN) && - buf_header->base.ep == PP_HTONL(1)) - { - if (dap_respond > 0) - { - 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 (dap_req_num > 0) { + DapPacket_t *item; + size_t packetSize = 0; + item = (DapPacket_t *)xRingbufferReceiveUpTo(dap_dataOUT_handle, &packetSize, + portMAX_DELAY, 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) - { - --dap_respond; - xSemaphoreGive(data_response_mux); - } + if (xSemaphoreTake(data_response_mux, portMAX_DELAY) == pdTRUE) { + --dap_respond; + xSemaphoreGive(data_response_mux); + } - 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); + 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; } + return 0; } diff --git a/main/DAP_handle.h b/main/DAP_handle.h index a7871df..d32979c 100644 --- a/main/DAP_handle.h +++ b/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 \ No newline at end of file diff --git a/main/tcp_server.c b/main/tcp_server.c index 2e44308..dd990e8 100644 --- a/main/tcp_server.c +++ b/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) - { - int len = recv(kSock, tcp_rx_buffer, sizeof(tcp_rx_buffer), 0); - // Error occured during receiving - if (len < 0) - { - os_printf("recv failed: errno %d\r\n", errno); - break; - } - // Connection closed - else if (len == 0) - { - os_printf("Connection closed\r\n"); - break; - } - // Data received + // Read header + sz = 4; + data = &tcp_rx_buffer[0]; + do { + ret = recv(kSock, data, sz, 0); + if (ret <= 0) + goto cleanup; + sz -= ret; + data += ret; + } while (sz > 0); + + header = *((int *)(tcp_rx_buffer)); + header = ntohl(header); + + if (header == EL_LINK_IDENTIFIER) { + 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 - { - // #ifdef CONFIG_EXAMPLE_IPV6 - // // Get the sender's ip address as string - // if (sourceAddr.sin6_family == PF_INET) - // { - // 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"); - } - } + usbip_state = WAIT_IMPORT; + usbip_worker(tcp_rx_buffer, sizeof(tcp_rx_buffer), &usbip_state); + } else { + os_printf("Unknown protocol\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(); diff --git a/main/usbip_server.c b/main/usbip_server.c index e41c00f..585b6a6 100644 --- a/main/usbip_server.c +++ b/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); } -/////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////// - -int emulate(uint8_t *buffer, uint32_t length) +static int usbip_urb_process(uint8_t *base, 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)) - { - return 0; + while (1) { + // header + 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); - if (command < 0) - { - return -1; - } - - 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; +out: + if (ret < 0) + os_printf("recv failed: errno %d\r\n", errno); + return ret; } -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)) - { - return -1; + uint8_t *data; + int pre_read_sz = 4; + 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)); - unpack((uint32_t *)header, sizeof(usbip_stage2_header)); - return header->base.command; + *state = WAIT_IMPORT; + // OP_REQ_IMPORT + 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); -} \ No newline at end of file +} diff --git a/main/usbip_server.h b/main/usbip_server.h index 5bfc296..0925ec7 100644 --- a/main/usbip_server.h +++ b/main/usbip_server.h @@ -5,18 +5,16 @@ #include "components/USBIP/usbip_defs.h" -enum state_t +enum usbip_server_state_t { - ACCEPTING, - ATTACHING, - EMULATING, - EL_DATA_PHASE + WAIT_DEVLIST = 0, + WAIT_IMPORT, + WAIT_URB, }; -extern uint8_t kState; + extern int kSock; -int attach(uint8_t *buffer, uint32_t length); -int emulate(uint8_t *buffer, uint32_t length); +int usbip_worker(uint8_t *base, uint32_t length, enum usbip_server_state_t *state); 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);