From af15799ccd0db0f99e36785fb19024ba530628ff Mon Sep 17 00:00:00 2001
From: Anton Fedorov <datacompboy@gmail.com>
Date: Tue, 24 Dec 2024 13:57:50 +0100
Subject: [PATCH] Fix pairing procedure handling

---
 prj.conf                             |  2 +-
 scripts/install/clone-kat-device.ps1 |  2 +-
 src/kat_usb_pack.c                   | 19 ++++++++++++++-----
 3 files changed, 16 insertions(+), 7 deletions(-)

diff --git a/prj.conf b/prj.conf
index 4a80582..380cae9 100644
--- a/prj.conf
+++ b/prj.conf
@@ -44,7 +44,7 @@ CONFIG_USB_DEVICE_PRODUCT="nRF KAT-VR Receiver"
 CONFIG_USB_DEVICE_VID=0xC4F4
 CONFIG_USB_DEVICE_PID=0x2F37
 # Kat libraries support SN length up to 12 chars with 11 being used now.
-CONFIG_USB_DEVICE_SN="ITWILLCHANGE"
+CONFIG_USB_DEVICE_SN="TEMPLATE11c"
 
 # Enable settings
 CONFIG_NVS=y
diff --git a/scripts/install/clone-kat-device.ps1 b/scripts/install/clone-kat-device.ps1
index 83824a1..c2c3048 100644
--- a/scripts/install/clone-kat-device.ps1
+++ b/scripts/install/clone-kat-device.ps1
@@ -101,7 +101,7 @@ if ($dev.serialNumber -ne $newSn) {
 # Write the pairing info
 $pairing=[IBizLibrary.KATSDKInterfaceHelper]::receiverPairingInfoSave.ReceiverPairingByte
 [byte[]]$ans = New-Object byte[] 32
-[byte[]]$command = 0x20,0x1f,0x55,0xAA,0x00,0x00,0x20,0x00 + $pairing + $ans
+[byte[]]$command = 0x20,0x1f,0x55,0xAA,0x00,0x00,0x20 + $pairing + $ans
 [IBizLibrary.KATSDKInterfaceHelper]::SendHIDCommand($dev.serialNumber, $command, 32, $ans, 32)
 
 Write-Host "Congratulations! The device is ready to use."
diff --git a/src/kat_usb_pack.c b/src/kat_usb_pack.c
index db3d209..98fdc13 100644
--- a/src/kat_usb_pack.c
+++ b/src/kat_usb_pack.c
@@ -46,6 +46,13 @@ typedef __PACKED_UNION
             }
             ansaddrs;
             __PACKED_STRUCT
+            {
+                uint8_t count;
+                uint8_t addrs[4][6]; // maximum 4 child MACs fits
+                uint8_t zero;
+            }
+            writeaddrs; // Differs from 'ansaddrs', there is no zero byte before count!
+            __PACKED_STRUCT
             {
                 uint8_t sensor_id;
                 uint8_t sensor_type;
@@ -195,7 +202,7 @@ bool kat_usb_handle_request(uint8_t *buf, int size)
 
     case cGetSN:
         pack->data.ansstring.zero = 0;
-        strncpy(pack->data.ansstring.ans, CONFIG_USB_DEVICE_SN, KAT_USB_PACKET_LEN - 8);
+        strncpy(pack->data.ansstring.ans, katUsbSerial, KAT_USB_PACKET_LEN - 8);
         pack->data.ansstring.ans[sizeof(pack->data.ansstring.ans) - 1] = 0; // ensure null termination
         return true;
 
@@ -224,16 +231,18 @@ bool kat_usb_handle_request(uint8_t *buf, int size)
         return true;
 
     case cWritePairing:
-        const int wcnt = MIN(ARRAY_SIZE(KatBleDevices), pack->data.ansaddrs.count);
+        const int wcnt = MIN(ARRAY_SIZE(KatBleDevices), pack->data.writeaddrs.count);
         NumKatBleDevices = wcnt;
         for (int i = 0; i < wcnt; ++i)
         {
-            BUILD_ASSERT(sizeof(pack->data.ansaddrs.addrs[i]) == sizeof(KatBleDevices[i].a), "Size of BT address should match");
-            memcpy(&KatBleDevices[i].a, &pack->data.ansaddrs.addrs[i], sizeof(KatBleDevices[i].a));
+            BUILD_ASSERT(sizeof(pack->data.writeaddrs.addrs[i]) == sizeof(KatBleDevices[i].a), "Size of BT address should match");
+            memcpy(&KatBleDevices[i].a, &pack->data.writeaddrs.addrs[i], sizeof(KatBleDevices[i].a));
         }
         kat_ble_update_devices();
         kat_settings_async_save();
-        return false;
+        // original receiver returns only command confirmation packet, no data
+        memset(&pack->data.writeaddrs, 0, sizeof(pack->data.writeaddrs));
+        return true;
 
     case cDeepSleep:
         return false;