From 40778a7dc692f407814e50ecf00c744c01d5f1b3 Mon Sep 17 00:00:00 2001 From: windowsair Date: Fri, 22 Jul 2022 19:04:47 +0800 Subject: [PATCH] feat: Add elaphureLink support --- components/elaphureLink/CMakeLists.txt | 4 ++ .../elaphureLink/elaphureLink_protocol.c | 68 +++++++++++++++++++ .../elaphureLink/elaphureLink_protocol.h | 53 +++++++++++++++ main/DAP_handle.c | 61 +++++++++++++---- main/DAP_handle.h | 7 ++ main/tcp_server.c | 21 +++++- main/usbip_server.h | 3 +- 7 files changed, 202 insertions(+), 15 deletions(-) create mode 100644 components/elaphureLink/CMakeLists.txt create mode 100644 components/elaphureLink/elaphureLink_protocol.c create mode 100644 components/elaphureLink/elaphureLink_protocol.h diff --git a/components/elaphureLink/CMakeLists.txt b/components/elaphureLink/CMakeLists.txt new file mode 100644 index 0000000..d522db1 --- /dev/null +++ b/components/elaphureLink/CMakeLists.txt @@ -0,0 +1,4 @@ +set(COMPONENT_ADD_INCLUDEDIRS ".") +set(COMPONENT_SRCS "./elaphureLink_protocol.c") + +register_component() \ No newline at end of file diff --git a/components/elaphureLink/elaphureLink_protocol.c b/components/elaphureLink/elaphureLink_protocol.c new file mode 100644 index 0000000..c86db73 --- /dev/null +++ b/components/elaphureLink/elaphureLink_protocol.c @@ -0,0 +1,68 @@ +#include "components/elaphureLink/elaphureLink_protocol.h" + + +#include "lwip/err.h" +#include "lwip/sockets.h" +#include "lwip/sys.h" +#include + +extern int kSock; +extern int usbip_network_send(int s, const void *dataptr, size_t size, int flags); + +extern void malloc_dap_ringbuf(); +extern void free_dap_ringbuf(); + +extern uint32_t DAP_ExecuteCommand(const uint8_t *request, uint8_t *response); + +uint8_t* el_process_buffer = NULL; + +void el_process_buffer_malloc() { + if (el_process_buffer != NULL) + return; + + free_dap_ringbuf(); + + el_process_buffer = malloc(1500); +} + + +void el_process_buffer_free() { + if (el_process_buffer != NULL) { + free(el_process_buffer); + el_process_buffer = NULL; + } +} + + +int el_handshake_process(int fd, void *buffer, size_t len) { + if (len != sizeof(el_request_handshake)) { + return -1; + } + + el_request_handshake* req = (el_request_handshake*)buffer; + + if (ntohl(req->el_link_identifier) != EL_LINK_IDENTIFIER) { + return -1; + } + + if (ntohl(req->command) != EL_COMMAND_HANDSHAKE) { + return -1; + } + + el_response_handshake res; + res.el_link_identifier = htonl(EL_LINK_IDENTIFIER); + res.command = htonl(EL_COMMAND_HANDSHAKE); + res.el_dap_version = htonl(EL_DAP_VERSION); + + usbip_network_send(fd, &res, sizeof(el_response_handshake), 0); + + 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); +} diff --git a/components/elaphureLink/elaphureLink_protocol.h b/components/elaphureLink/elaphureLink_protocol.h new file mode 100644 index 0000000..751c024 --- /dev/null +++ b/components/elaphureLink/elaphureLink_protocol.h @@ -0,0 +1,53 @@ +#ifndef __ELAPHURELINK_PROTOCOL_H__ +#define __ELAPHURELINK_PROTOCOL_H__ + +#include +#include + +#define EL_LINK_IDENTIFIER 0x8a656c70 + +#define EL_DAP_VERSION 0x00000001 + +#define EL_COMMAND_HANDSHAKE 0x00000000 + + +typedef struct +{ + uint32_t el_link_identifier; + uint32_t command; + uint32_t el_proxy_version +} __attribute__((packed)) el_request_handshake; + + +typedef struct +{ + uint32_t el_link_identifier; + uint32_t command; + uint32_t el_dap_version +} __attribute__((packed)) el_response_handshake; + + +/** + * @brief elahpureLink Proxy handshake phase process + * + * @param fd socket fd + * @param buffer packet buffer + * @param len packet length + * @return 0 on Success, other on failed. + */ +int el_handshake_process(int fd, void* buffer, size_t len); + + +/** + * @brief Process dap data and send to socket + * + * @param buffer dap data buffer + * @param len dap data length + */ +void el_dap_data_process(void* buffer, size_t len); + + +void el_process_buffer_malloc(); +void el_process_buffer_free(); + +#endif \ No newline at end of file diff --git a/main/DAP_handle.c b/main/DAP_handle.c index 962cc22..373cfff 100644 --- a/main/DAP_handle.c +++ b/main/DAP_handle.c @@ -58,7 +58,7 @@ typedef struct extern int kSock; extern TaskHandle_t kDAPTaskHandle; -int kRestartDAPHandle = 0; +int kRestartDAPHandle = NO_SIGNAL; static DapPacket_t DAPDataProcessed; @@ -74,6 +74,36 @@ static RingbufHandle_t dap_dataOUT_handle = NULL; static SemaphoreHandle_t data_response_mux = NULL; +void malloc_dap_ringbuf() { + if (data_response_mux && xSemaphoreTake(data_response_mux, portMAX_DELAY) == pdTRUE) + { + if (dap_dataIN_handle == NULL) { + dap_dataIN_handle = xRingbufferCreate(DAP_HANDLE_SIZE * DAP_BUFFER_NUM, RINGBUF_TYPE_BYTEBUF); + } + if (dap_dataOUT_handle == NULL) { + dap_dataOUT_handle = xRingbufferCreate(DAP_HANDLE_SIZE * DAP_BUFFER_NUM, RINGBUF_TYPE_BYTEBUF); + } + + xSemaphoreGive(data_response_mux); + } +} + +void free_dap_ringbuf() { + if (data_response_mux && xSemaphoreTake(data_response_mux, portMAX_DELAY) == pdTRUE) { + if (dap_dataIN_handle) { + vRingbufferDelete(dap_dataIN_handle); + } + if (dap_dataOUT_handle) { + vRingbufferDelete(dap_dataOUT_handle); + } + + dap_dataIN_handle = dap_dataOUT_handle = NULL; + xSemaphoreGive(data_response_mux); + } + +} + + void handle_dap_data_request(usbip_stage2_header *header, uint32_t length) { uint8_t *data_in = (uint8_t *)header; @@ -168,21 +198,28 @@ void DAP_Thread(void *argument) { if (kRestartDAPHandle) { - vRingbufferDelete(dap_dataIN_handle); - vRingbufferDelete(dap_dataOUT_handle); - dap_dataIN_handle = dap_dataOUT_handle = NULL; + free_dap_ringbuf(); - dap_dataIN_handle = xRingbufferCreate(DAP_HANDLE_SIZE * DAP_BUFFER_NUM, RINGBUF_TYPE_BYTEBUF); - dap_dataOUT_handle = xRingbufferCreate(DAP_HANDLE_SIZE * DAP_BUFFER_NUM, RINGBUF_TYPE_BYTEBUF); - if (dap_dataIN_handle == NULL || dap_dataIN_handle == NULL) - { - os_printf("Can not create DAP ringbuf/mux!\r\n"); - vTaskDelete(NULL); + if (kRestartDAPHandle == RESET_HANDLE) { + malloc_dap_ringbuf(); + if (dap_dataIN_handle == NULL || dap_dataIN_handle == NULL) + { + os_printf("Can not create DAP ringbuf/mux!\r\n"); + vTaskDelete(NULL); + } } - kRestartDAPHandle = 0; + + kRestartDAPHandle = NO_SIGNAL; } - ulTaskNotifyTake(pdFALSE, portMAX_DELAY); + ulTaskNotifyTake(pdFALSE, portMAX_DELAY); // wait event + + + if (dap_dataIN_handle == NULL || dap_dataOUT_handle == NULL) { + continue; // may be use elaphureLink, wait... + } + + packetSize = 0; item = (DapPacket_t *)xRingbufferReceiveUpTo(dap_dataIN_handle, &packetSize, pdMS_TO_TICKS(1), DAP_HANDLE_SIZE); diff --git a/main/DAP_handle.h b/main/DAP_handle.h index 4d75e6e..a7871df 100644 --- a/main/DAP_handle.h +++ b/main/DAP_handle.h @@ -3,6 +3,13 @@ #include "components/USBIP/usbip_defs.h" +enum reset_handle_t +{ + NO_SIGNAL = 0, + RESET_HANDLE = 1, + DELETE_HANDLE = 2, +}; + 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); diff --git a/main/tcp_server.c b/main/tcp_server.c index 1e87ec9..2e44308 100644 --- a/main/tcp_server.c +++ b/main/tcp_server.c @@ -13,6 +13,9 @@ #include "main/wifi_configuration.h" #include "main/usbip_server.h" +#include "main/DAP_handle.h" + +#include "components/elaphureLink/elaphureLink_protocol.h" #include "freertos/FreeRTOS.h" #include "freertos/task.h" @@ -146,12 +149,24 @@ void tcp_server_task(void *pvParameters) 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"); } @@ -163,11 +178,13 @@ void tcp_server_task(void *pvParameters) os_printf("Shutting down socket and restarting...\r\n"); //shutdown(kSock, 0); close(kSock); - if (kState == EMULATING) + if (kState == EMULATING || kState == EL_DATA_PHASE) kState = ACCEPTING; // Restart DAP Handle - kRestartDAPHandle = 1; + el_process_buffer_free(); + + kRestartDAPHandle = RESET_HANDLE; if (kDAPTaskHandle) xTaskNotifyGive(kDAPTaskHandle); diff --git a/main/usbip_server.h b/main/usbip_server.h index af6be47..5bfc296 100644 --- a/main/usbip_server.h +++ b/main/usbip_server.h @@ -9,7 +9,8 @@ enum state_t { ACCEPTING, ATTACHING, - EMULATING + EMULATING, + EL_DATA_PHASE }; extern uint8_t kState; extern int kSock;