feat(dap): Add support for elaphureLink vendor command

Author: windowsair

components/DAP/source/DAP.c

@@ -1670,20 +1670,6 @@ static uint32_t DAP_WriteAbort(const uint8_t *request, uint8_t *response) {
   return ((5U << 16) | num);
 }
 
-
-// Process DAP Vendor command request and prepare response
-// Default function (can be overridden)
-//   request:  pointer to request data
-//   response: pointer to response data
-//   return:   number of bytes in response (lower 16 bits)
-//             number of bytes in request (upper 16 bits)
-__WEAK uint32_t DAP_ProcessVendorCommand(const uint8_t *request, uint8_t *response) {
-  (void)request;
-  *response = ID_DAP_Invalid;
-  return ((1U << 16) | 1U);
-}
-
-
 // Process DAP command request and prepare response
 //   request:  pointer to request data
 //   response: pointer to response data

components/DAP/source/DAP_vendor.c

@@ -27,6 +27,7 @@
 
 #include "components/DAP/config/DAP_config.h"
 #include "components/DAP/include/DAP.h"
+#include "components/elaphureLink/elaphureLink_protocol.h"
 
 //**************************************************************************************************
 /**
@@ -68,7 +69,9 @@ uint32_t DAP_ProcessVendorCommand(const uint8_t *request, uint8_t *response) {
     case ID_DAP_Vendor5:  break;
     case ID_DAP_Vendor6:  break;
     case ID_DAP_Vendor7:  break;
-    case ID_DAP_Vendor8:  break;
+    case ID_DAP_Vendor8:
+      num = el_vendor_command(request, response);
+      break;
     case ID_DAP_Vendor9:  break;
     case ID_DAP_Vendor10: break;
     case ID_DAP_Vendor11: break;

components/elaphureLink/elaphureLink_protocol.c

@@ -16,6 +16,11 @@ extern void free_dap_ringbuf();
 
 extern uint32_t DAP_ExecuteCommand(const uint8_t *request, uint8_t *response);
 
+struct el_context {
+    bool is_async;
+};
+
+static struct el_context k_el_context;
 uint8_t* el_process_buffer = NULL;
 
 void el_process_buffer_malloc() {
@@ -66,11 +71,83 @@ void el_dap_data_process(void* buffer, size_t len) {
     usbip_network_send(kSock, el_process_buffer, res, 0);
 }
 
+static inline int recv_all(int fd, uint8_t *buf, size_t size, int flag)
+{
+    const size_t total = size;
+    int ret;
+
+    if (size == 0)
+        return 0;
+
+    do {
+        ret = recv(fd, buf, size, flag);
+        if (ret <= 0)
+            return ret;
+
+        buf += ret;
+        size -= ret;
+    } while (size);
+
+    return total;
+}
+
+static int el_vendor_command_pre_process(uint8_t *base, int recved_len)
+{
+    int offset = 0;
+    int payload_len, remain_len, packet_len;
+    uint16_t *payload;
+    int ret;
+
+    while (recved_len - offset >= 4) {
+        payload = (uint16_t *)(base + offset + 2);
+        payload_len = ntohs(*payload);
+        packet_len = 4 + payload_len;
+
+        if (offset + packet_len > recved_len)
+            break;
+
+        el_dap_data_process(base + offset, packet_len);
+        offset += packet_len;
+    }
+
+    // already process done
+    remain_len = recved_len - offset;
+    if (remain_len == 0)
+        return 1;
+
+    memmove(base, base + offset, remain_len);
+    if (remain_len < 4) {
+        ret = recv(kSock, base + remain_len, 4 - remain_len, 0);
+        if (ret <= 0)
+            return ret;
+        offset = 4;
+        remain_len = 0;
+    } else {
+        offset = remain_len;
+        remain_len -= 4;
+    }
+
+    payload = (uint16_t *)(base + 2);
+    payload_len = ntohs(*payload);
+    if (payload_len - remain_len > 0) {
+        ret = recv(kSock, base + offset, payload_len - remain_len, 0);
+        if (ret <= 0)
+            return ret;
+    }
+
+    el_dap_data_process(base, 4 + payload_len);
+
+    return 1;
+}
+
 int el_dap_work(uint8_t* base, size_t len)
 {
+    uint16_t *length, payload_len;
     uint8_t *data;
     int sz, ret;
 
+    memset(&k_el_context, 0, sizeof(struct el_context));
+
     // read command code and protocol version
     data = base + 4;
     sz = 8;
@@ -93,8 +170,80 @@ int el_dap_work(uint8_t* base, size_t len)
         ret = recv(kSock, base, len, 0);
         if (ret <= 0)
             return ret;
-        el_dap_data_process(base, ret);
+
+        if (*base == EL_VENDOR_COMMAND_PERFIX) {
+            ret = el_vendor_command_pre_process(base, ret);
+            if (ret <= 0)
+                return ret;
+        } else {
+            el_dap_data_process(base, ret);
+        }
+
+        if (k_el_context.is_async) {
+            do {
+                ret = recv_all(kSock, base, 4, 0);
+                if (ret <= 0)
+                    return ret;
+                length = (uint16_t *)(base + 2);
+                payload_len = ntohs(*length);
+
+                ret = recv_all(kSock, base + 4, payload_len, 0);
+                if (ret <= 0)
+                    return ret;
+                el_dap_data_process(base, 4 + ntohs(*length));
+            } while (k_el_context.is_async);
+        }
     }
 
     return 0;
 }
+
+uint32_t el_native_command_passthrough(const uint8_t *request, uint8_t *response)
+{
+    int ret;
+
+    request += 2; // skip header (length field)
+
+    ret = DAP_ExecuteCommand(request, response + 3);
+    ret &= 0xFFFF;
+
+    response[0] = 0x00; // status
+    response[1] = (ret >> 8) & 0xFF;
+    response[2] = ret & 0xFF;
+
+    return ret + 4; // header + payload
+}
+
+uint32_t el_vendor_command(const uint8_t *request, uint8_t *response)
+{
+    uint8_t type;
+    uint32_t ret = 0;
+
+    type = *request++;
+
+    switch (type) {
+    case EL_NATIVE_COMMAND_PASSTHROUGH:
+        ret = el_native_command_passthrough(request, response);
+        break;
+    case EL_VENDOR_SCOPE_ENTER:
+        memset(&k_el_context, 0, sizeof(struct el_context));
+        k_el_context.is_async = true;
+        *response++ = 0; // status
+        *response++ = 0;
+        *response++ = 0;
+        ret = 4;
+        break;
+    case EL_VENDOR_SCOPE_EXIT:
+        k_el_context.is_async = false;
+        *response++ = 0; // status
+        *response++ = 0;
+        *response++ = 0;
+        ret = 4;
+        break;
+    default:
+        break;
+    }
+
+    return ret;
+}
+

components/elaphureLink/elaphureLink_protocol.h

@@ -6,10 +6,14 @@
 
 #define EL_LINK_IDENTIFIER 0x8a656c70
 
-#define EL_DAP_VERSION 0x00000001
+#define EL_DAP_VERSION 0x10000 // v1.0.0 support vendor command
 
 #define EL_COMMAND_HANDSHAKE 0x00000000
 
+#define EL_VENDOR_COMMAND_PERFIX        0x88
+#define EL_NATIVE_COMMAND_PASSTHROUGH   0x1
+#define EL_VENDOR_SCOPE_ENTER           0x2
+#define EL_VENDOR_SCOPE_EXIT            0x3
 
 typedef struct
 {
@@ -47,6 +51,8 @@ int el_handshake_process(int fd, void* buffer, size_t len);
 void el_dap_data_process(void* buffer, size_t len);
 
 
+uint32_t el_vendor_command(const uint8_t *request, uint8_t *response);
+
 int el_dap_work(uint8_t* base, size_t len);
 
 void el_process_buffer_malloc();