From 7f67490c96dde1b9eea658b10d6852bc85e065e3 Mon Sep 17 00:00:00 2001
From: Matthias Ringwald <matthias@ringwald.ch>
Date: Tue, 6 Jul 2021 14:52:23 +0200
Subject: [PATCH] hci_cmd: support variable len arguments with 'JV'

---
 src/hci_cmd.c                | 20 ++++++++++++++++++++
 test/embedded/hci_cmd_test.c | 21 +++++++++++++++------
 2 files changed, 35 insertions(+), 6 deletions(-)

diff --git a/src/hci_cmd.c b/src/hci_cmd.c
index 4221c04b3..8914aa030 100644
--- a/src/hci_cmd.c
+++ b/src/hci_cmd.c
@@ -59,6 +59,8 @@
 // calculate combined ogf/ocf value
 #define OPCODE(ogf, ocf) ((ocf) | ((ogf) << 10))
 
+#define INVALID_VAR_LEN 0xffffu
+
 /**
  * construct HCI Command based on template
  *
@@ -73,6 +75,8 @@
  *   A: 31 bytes advertising data
  *   S: Service Record (Data Element Sequence)
  *   Q: 32 byte data block, e.g. for X and Y coordinates of P-256 public key
+ *   J: 8-bit length of variable size element
+ *   V: variable size element, len was given with param 'J'
  */
 uint16_t hci_cmd_create_from_template(uint8_t *hci_cmd_buffer, const hci_cmd_t *cmd, va_list argptr){
     
@@ -84,6 +88,8 @@ uint16_t hci_cmd_create_from_template(uint8_t *hci_cmd_buffer, const hci_cmd_t *
     uint16_t word;
     uint32_t longword;
     uint8_t * ptr;
+    uint16_t var_len = INVALID_VAR_LEN;
+
     while (*format) {
         switch(*format) {
             case '1': //  8 bit value
@@ -91,6 +97,12 @@ uint16_t hci_cmd_create_from_template(uint8_t *hci_cmd_buffer, const hci_cmd_t *
                 word = va_arg(argptr, int); // LCOV_EXCL_BR_LINE
                 hci_cmd_buffer[pos++] = word & 0xffu;
                 break;
+            case 'J': //  8 bit variable length indicator
+                // minimal va_arg is int: 2 bytes on 8+16 bit CPUs
+                word = va_arg(argptr, int); // LCOV_EXCL_BR_LINE
+                var_len = word & 0xffu;
+                hci_cmd_buffer[pos++] = var_len;
+                break;
             case '2': // 16 bit value
                 // minimal va_arg is int: 2 bytes on 8+16 bit CPUs
                 word = va_arg(argptr, int); // LCOV_EXCL_BR_LINE
@@ -182,7 +194,15 @@ uint16_t hci_cmd_create_from_template(uint8_t *hci_cmd_buffer, const hci_cmd_t *
                 reverse_bytes(ptr, &hci_cmd_buffer[pos], 16);
                 pos += 16;
                 break;
+            case 'V':
+                btstack_assert(var_len != INVALID_VAR_LEN);
+                ptr = va_arg(argptr, uint8_t *); // LCOV_EXCL_BR_LINE
+                (void)memcpy(&hci_cmd_buffer[pos], ptr, var_len);
+                pos += var_len;
+                var_len = INVALID_VAR_LEN;
+                break;
             default:
+                btstack_unreachable();
                 break;
         }
         format++;
diff --git a/test/embedded/hci_cmd_test.c b/test/embedded/hci_cmd_test.c
index 560e868e3..a56ef72bc 100644
--- a/test/embedded/hci_cmd_test.c
+++ b/test/embedded/hci_cmd_test.c
@@ -2,6 +2,7 @@
 #include "CppUTest/CommandLineTestRunner.h"
 
 #include "hci_cmd.h"
+#include "btstack_util.h"
 
 static uint8_t hci_cmd_buffer[350];
 static uint8_t input_buffer[350];
@@ -14,12 +15,6 @@ static uint16_t create_hci_cmd(const hci_cmd_t *cmd, ...){
     return len - 3;
 }
 
-void reverse_bytes(const uint8_t * src, uint8_t * dest, int len){
-    int i;
-    for (i = 0; i < len; i++)
-        dest[len - 1 - i] = src[i];
-}
-
 const hci_cmd_t cmd_1 = {0x1234, "1"};
 const hci_cmd_t cmd_2 = {0x1234, "2"};
 const hci_cmd_t cmd_3 = {0x1234, "3"};
@@ -33,6 +28,7 @@ const hci_cmd_t cmd_K = {0x1234, "K"};
 const hci_cmd_t cmd_N = {0x1234, "N"};
 const hci_cmd_t cmd_P = {0x1234, "P"};
 const hci_cmd_t cmd_Q = {0x1234, "Q"};
+const hci_cmd_t cmd_JV = {0x1234, "JV"};
 
 const hci_cmd_t cmd_INVALID = {0x1234, "!"};
 
@@ -183,6 +179,19 @@ TEST(HCI_Command, format_Q){
     CHECK_EQUAL(expected_size, size);
     MEMCMP_EQUAL(expected_buffer, &hci_cmd_buffer[3], expected_size);
 }
+TEST(HCI_Command, format_JV){
+    uint8_t expected_buffer[4];
+    uint16_t expected_size = 4;
+    uint8_t var_len_arg = expected_size - 1;
+    setup_input_buffer(var_len_arg);
+    expected_buffer[0] = var_len_arg;
+    memcpy(&expected_buffer[1], input_buffer, var_len_arg);
+
+    uint16_t size = create_hci_cmd(&cmd_JV, var_len_arg, input_buffer);
+    printf_hexdump(hci_cmd_buffer, size + 3);
+    CHECK_EQUAL(expected_size, size);
+    MEMCMP_EQUAL(expected_buffer, &hci_cmd_buffer[3], expected_size);
+}
 
 TEST(HCI_Command, format_INVALID){
     uint16_t expected_size = 0;