kerms
/
wireless-proxy-esp32
0
0
Fork 0
Code

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.
This commit is contained in:
windowsair 2024-02-16 18:46:50 +08:00
parent 8abe565836
commit 16f8b53a76
8 changed files with 219 additions and 243 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
components/elaphureLink/elaphureLink_protocol.c
View File

@ -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;
}

2
components/elaphureLink/elaphureLink_protocol.h
View File

@ -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();

2
main/CMakeLists.txt
View File

@ -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()

85
main/DAP_handle.c
View File

@ -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;
}

3
main/DAP_handle.h
View File

@ -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

93
main/tcp_server.c
View File

@ -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();

223
main/usbip_server.c
View File

@ -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);
}

14
main/usbip_server.h
View File

@ -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);