use templates for HCI commands

src/hci.c

@@ -6,9 +6,20 @@
  */
 
 #include <unistd.h>
-
+#include <stdarg.h>
+#include <string.h>
 #include "hci.h"
 
+
+hci_cmd_t hci_inquiry = {
+    0x01, 0x01, "311" // LAP, Inquiry length, Num_responses
+};
+
+hci_cmd_t hci_reset = {
+    0x03, 0x03, ""
+};
+
+
 static hci_transport_t *hci_transport;
 
 void hci_init(hci_transport_t *transport, void *config){
@@ -29,7 +40,7 @@ int hci_power_control(HCI_POWER_MODE power_mode){
     return 0;
 }
 
-int hci_send_cmd(char *buffer, int size){
+int hci_send_cmd(uint8_t *buffer, int size){
     return hci_transport->send_cmd_packet(buffer, size);
 }
 
@@ -41,4 +52,55 @@ void hci_run(){
         // for each ready file in FD - call handle_data
         sleep(1);
     }
+}
+
+
+void hci_create_cmd_packet(uint8_t *buffer, uint8_t *cmd_len, hci_cmd_t *cmd, ...){
+    buffer[0] = cmd->ocf;
+    buffer[1] = cmd->ocf >> 8 | cmd->ogf << 2;
+    int pos = 3;
+
+    va_list argptr;
+    va_start(argptr, cmd);
+    const char *format = cmd->format;
+    uint16_t word;
+    uint32_t longword;
+    uint8_t * bt_addr;
+    while (*format) {
+        switch(*format) {
+            case '1': //  8 bit value
+            case '2': // 16 bit value
+            case 'H': // hci_handle
+                word = va_arg(argptr, int);  // minimum c parameter width is int
+                buffer[pos++] = word & 0xff;
+                if (*format == '2') {
+                    buffer[pos++] = word >> 8;
+                } else if (*format == 'H') {
+                    
+                } 
+                break;
+            case '3':
+            case '4':
+                longword = va_arg(argptr, uint32_t);
+                // longword = va_arg(argptr, int);
+                buffer[pos++] = longword;
+                buffer[pos++] = longword >> 8;
+                buffer[pos++] = longword >> 16;
+                if (*format == '4'){
+                    buffer[pos++] = longword >> 24;
+                }
+                break;
+            case 'B': // bt-addr
+                bt_addr = va_arg(argptr, uint8_t *);
+                memcpy( &buffer[pos], bt_addr, 6);
+                pos += 6;
+                break;
+            default:
+                break;
+        }
+        format++;
+    };
+    va_end(argptr);
+    buffer[2] = pos - 3;
+    *cmd_len = pos;
 }

src/hci.h

@@ -7,6 +7,8 @@
 
 #pragma once
 
+#include <stdint.h>
+
 #include "hci_transport.h"
 
 typedef enum {
@@ -14,6 +16,12 @@ typedef enum {
     HCI_POWER_ON 
 } HCI_POWER_MODE;
 
+typedef struct {
+    uint8_t  ogf;
+    uint16_t ocf;
+    const char *format;
+} hci_cmd_t;
+
 // set up HCI
 void hci_init(hci_transport_t *transport, void *config);
 
@@ -23,3 +31,10 @@ int hci_power_control(HCI_POWER_MODE mode);
 // run the hci daemon loop
 void hci_run();
 
+// create hci command packet based on a template and a list of parameters
+void hci_create_cmd_packet(uint8_t *buffer, uint8_t *cmd_len, hci_cmd_t *cmd, ...);
+
+extern hci_cmd_t hci_inquiry;
+extern hci_cmd_t hci_reset;
+    
+#define HCI_INQUIRY_LAP 0x9E8B33L  // 0x9E8B33: General/Unlimited Inquiry Access Code (GIAC)

src/main.c

@@ -17,6 +17,7 @@
 
 static hci_transport_t * transport;
 static hci_uart_config_t config;
+static uint8_t buffer [200];
 
 #if 0
 static void *hci_daemon_thread(void *arg){
@@ -34,21 +35,28 @@ void hexdump(uint8_t *data, int size){
     printf("\n");
 }
 
+void dump_cmd(uint8_t *data, int size){
+    printf("CMD: ");
+    hexdump(data, size);
+}
+
 void event_handler(uint8_t *packet, int size){
-    printf("Event received, size %u\n", size );
+    printf("EVT: ");
     hexdump( packet, size);
     
     // 
     if (packet[3] == 3 && packet[4] == 12){
         // reset done, send inq
-        uint8_t inq[] = {  0x01, 0x04, 0x05, 0x33,  0x8b,  0x9e, 0x0a, 0x14};
-        transport->send_cmd_packet( inq, sizeof(inq) );
+        uint8_t len;
+        hci_create_cmd_packet( buffer, &len, &hci_inquiry, HCI_INQUIRY_LAP, 30, 0);
+        transport->send_cmd_packet( buffer, len );
+        dump_cmd( buffer, len);
     }
 }
 
 
 int main (int argc, const char * argv[]) {
-    
+        
     // 
     if (argc <= 1){
         printf("HCI Daemon tester. Specify device name for Ericsson ROK 101 007\n");
@@ -75,7 +83,7 @@ int main (int argc, const char * argv[]) {
     }
     
 #endif
-
+    
     // hci_init(transport, &config);
     // hci_power_control(HCI_POWER_ON);
     
@@ -92,9 +100,11 @@ int main (int argc, const char * argv[]) {
     int transport_fd = transport->get_fd();
     
     // send hci reset
-    uint8_t reset[] = { 0x03, 0x0c, 0x00 };
-    transport->send_cmd_packet( reset, sizeof(reset) );
-    
+    uint8_t len;
+    hci_create_cmd_packet( buffer, &len, &hci_reset);
+    transport->send_cmd_packet( buffer, len );
+    dump_cmd( buffer, len);
+
     // 
     fd_set descriptors;
     FD_ZERO(&descriptors);