feat: Add elaphureLink support

windowsair · Jul 22, 2022 · 40778a7 · 40778a7
1 parent 0f9f0eb
commit 40778a7
Show file tree

Hide file tree

Showing 7 changed files with 203 additions and 16 deletions.
diff --git a/components/elaphureLink/CMakeLists.txt b/components/elaphureLink/CMakeLists.txt
@@ -0,0 +1,4 @@
+set(COMPONENT_ADD_INCLUDEDIRS ".")
+set(COMPONENT_SRCS "./elaphureLink_protocol.c")
+
+register_component()
diff --git a/components/elaphureLink/elaphureLink_protocol.c 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 <lwip/netdb.h>
+
+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
@@ -0,0 +1,53 @@
+#ifndef __ELAPHURELINK_PROTOCOL_H__
+#define __ELAPHURELINK_PROTOCOL_H__
+
+#include <stdint.h>
+#include <stddef.h>
+
+#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
diff --git 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;
-
-                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);
+                free_dap_ringbuf();
+
+                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
@@ -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
@@ -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
@@ -9,7 +9,8 @@ enum state_t
 {
     ACCEPTING,
     ATTACHING,
-    EMULATING
+    EMULATING,
+    EL_DATA_PHASE
 };
 extern uint8_t kState;
 extern int kSock;