From d0f26bf0c78bf52a56173adb9e57b021e5ec80cc Mon Sep 17 00:00:00 2001
From: Matthias Ringwald <matthias@ringwald.ch>
Date: Fri, 7 Apr 2023 18:33:48 +0200
Subject: [PATCH] chipset/nxp: send HCI_CMD_SET_SCO_DATA_PATH command

---
 chipset/nxp/btstack_chipset_nxp.c | 39 ++++++++++++++++++++++++++++---
 1 file changed, 36 insertions(+), 3 deletions(-)

diff --git a/chipset/nxp/btstack_chipset_nxp.c b/chipset/nxp/btstack_chipset_nxp.c
index 1a2a3d5f4..0a037fd4d 100644
--- a/chipset/nxp/btstack_chipset_nxp.c
+++ b/chipset/nxp/btstack_chipset_nxp.c
@@ -72,6 +72,10 @@
 
 #define NXP_MAX_RESEND_COUNT 5
 
+// vendor commands
+#define NXP_OPCODE_SET_SCO_DATA_PATH	0xFC1D
+#define NXP_OPCODE_SET_BDADDR		    0xFC22
+
 // prototypes
 static void nxp_send_chunk_v1(void);
 static void nxp_done_with_status(uint8_t status);
@@ -404,15 +408,44 @@ void btstack_chipset_nxp_download_firmware_with_uart(const btstack_uart_t *uart_
     nxp_start();
 }
 
+// init script support
+static enum {
+    NXP_INIT_SEND_SCO_CONFIG,
+    NXP_INIT_DONE,
+} nxp_init_state;
 
-static void chipset_init(const void *transport_config){
+static void nxp_init(const void *transport_config){
     UNUSED(transport_config);
+    nxp_init_state = NXP_INIT_SEND_SCO_CONFIG;
+}
+
+static btstack_chipset_result_t nxp_next_command(uint8_t * hci_cmd_buffer) {
+    switch (nxp_init_state){
+        case NXP_INIT_SEND_SCO_CONFIG:
+#if defined(ENABLE_SCO_OVER_HCI) || defined(ENABLE_SCO_OVER_PCM)
+            little_endian_store_16(hci_cmd_buffer, 0, NXP_OPCODE_SET_SCO_DATA_PATH);
+            hci_cmd_buffer[2] = 1;
+#ifdef ENABLE_SCO_OVER_HCI
+            // Voice Path: Host
+            hci_cmd_buffer[3] = 0;
+#else
+            // Voice Path: PCM/I2S
+            hci_cmd_buffer[3] = 1;
+#endif
+            nxp_init_state = NXP_INIT_DONE;
+            return BTSTACK_CHIPSET_VALID_COMMAND;
+#endif
+            break;
+        case NXP_INIT_DONE:
+            break;
+    }
+    return BTSTACK_CHIPSET_DONE;
 }
 
 static btstack_chipset_t btstack_chipset_nxp = {
         .name = "NXP",
-        .init = chipset_init,
-        .next_command = NULL,
+        .init = nxp_init,
+        .next_command = nxp_next_command,
         .set_baudrate_command = NULL,
         .set_bd_addr_command = NULL
 };