feat(core/prodtest): add BLE testing

[no changelog]

core/SConscript.prodtest

@@ -10,7 +10,7 @@ BOOTLOADER_DEVEL = ARGUMENTS.get('BOOTLOADER_DEVEL', '0') == '1'
 HW_REVISION = ARGUMENTS.get('HW_REVISION', None)
 UI_DEBUG_OVERLAY = ARGUMENTS.get('UI_DEBUG_OVERLAY', '0') == '1'
 
-FEATURES_WANTED = ["input", "sbu", "nfc", "sd_card", "rgb_led", "usb", "consumption_mask", "optiga", "haptic", "tropic"]
+FEATURES_WANTED = ["input", "sbu", "nfc", "sd_card", "rgb_led", "usb", "consumption_mask", "optiga", "haptic", "tropic", "ble"]
 
 CCFLAGS_MOD = ''
 CPPPATH_MOD = []
@@ -100,6 +100,7 @@ SOURCE_PRODTEST = [
     'embed/projects/prodtest/header.S',
     'embed/projects/prodtest/main.c',
     'embed/projects/prodtest/cmd/prodtest_boardloader.c',
+    'embed/projects/prodtest/cmd/prodtest_ble.c',
     'embed/projects/prodtest/cmd/prodtest_bootloader.c',
     'embed/projects/prodtest/cmd/prodtest_button.c',
     'embed/projects/prodtest/cmd/prodtest_display.c',

core/embed/io/ble/inc/io/ble.h

@@ -41,10 +41,14 @@ typedef enum {
   BLE_REJECT_PAIRING = 6,  // Reject pairing request
 } ble_command_type_t;
 
+typedef struct {
+  uint8_t name[BLE_ADV_NAME_LEN];
+  bool static_mac;
+} ble_adv_start_cmd_data_t;
+
 typedef union {
   uint8_t raw[32];
-  uint8_t name[BLE_ADV_NAME_LEN];
-
+  ble_adv_start_cmd_data_t adv_start;
 } ble_command_data_t;
 
 typedef struct {
@@ -73,6 +77,7 @@ typedef struct {
   bool connectable;
   bool pairing;
   bool pairing_requested;
+  bool state_known;
   uint8_t peer_count;
 } ble_state_t;
 
@@ -139,4 +144,10 @@ bool ble_can_read(void);
 // Returns the number of bytes actually read.
 uint32_t ble_read(uint8_t *data, uint16_t max_len);
 
+// Read MAC address of the device
+//
+// When not using static address, the address is random and may not correspond
+// to what is actually used for advertising
+bool ble_get_mac(uint8_t *mac, size_t max_len);
+
 #endif

core/embed/io/ble/stm32/ble.c

@@ -24,6 +24,7 @@
 #include <io/ble.h>
 #include <io/nrf.h>
 #include <sys/irq.h>
+#include <sys/systick.h>
 #include <sys/systimer.h>
 #include <util/tsqueue.h>
 #include <util/unit_properties.h>
@@ -66,7 +67,9 @@ typedef struct {
   tsqueue_entry_t ts_queue_entries[TX_QUEUE_LEN];
   tsqueue_t tx_queue;
 
-  char adv_name[BLE_ADV_NAME_LEN];
+  ble_adv_start_cmd_data_t adv_cmd;
+  uint8_t mac[6];
+  bool mac_ready;
   systimer_t *timer;
   uint16_t ping_cntr;
 } ble_driver_t;
@@ -90,8 +93,11 @@ static bool ble_send_advertising_on(ble_driver_t *drv, bool whitelist) {
       .cmd_id = INTERNAL_CMD_ADVERTISING_ON,
       .whitelist = whitelist ? 1 : 0,
       .color = props.color,
+      .static_addr = drv->adv_cmd.static_mac,
+      .device_code = HW_MODEL,
   };
-  memcpy(data.name, drv->adv_name, BLE_ADV_NAME_LEN);
+
+  memcpy(data.name, drv->adv_cmd.name, BLE_ADV_NAME_LEN);
 
   return nrf_send_msg(NRF_SERVICE_BLE_MANAGER, (uint8_t *)&data, sizeof(data),
                       NULL, NULL) >= 0;
@@ -142,6 +148,13 @@ static bool ble_send_pairing_accept(ble_driver_t *drv) {
   return result;
 }
 
+static bool ble_send_mac_request(ble_driver_t *drv) {
+  (void)drv;
+  uint8_t cmd = INTERNAL_CMD_MAC_REQUEST;
+
+  return nrf_send_msg(NRF_SERVICE_BLE_MANAGER, &cmd, sizeof(cmd), NULL, NULL);
+}
+
 static void ble_process_rx_msg_status(const uint8_t *data, uint32_t len) {
   ble_driver_t *drv = &g_ble_driver;
 
@@ -233,6 +246,16 @@ static void ble_process_rx_msg_pairing_cancelled(const uint8_t *data,
   drv->pairing_requested = false;
 }
 
+static void ble_process_rx_msg_mac(const uint8_t *data, uint32_t len) {
+  ble_driver_t *drv = &g_ble_driver;
+  if (!drv->initialized) {
+    return;
+  }
+
+  drv->mac_ready = true;
+  memcpy(drv->mac, &data[1], sizeof(drv->mac));
+}
+
 static void ble_process_rx_msg(const uint8_t *data, uint32_t len) {
   if (len < 1) {
     return;
@@ -248,6 +271,8 @@ static void ble_process_rx_msg(const uint8_t *data, uint32_t len) {
     case INTERNAL_EVENT_PAIRING_CANCELLED:
       ble_process_rx_msg_pairing_cancelled(data, len);
       break;
+    case INTERNAL_EVENT_MAC:
+      ble_process_rx_msg_mac(data, len);
     default:
       break;
   }
@@ -525,11 +550,11 @@ bool ble_issue_command(ble_command_t *command) {
       drv->mode_requested = BLE_MODE_OFF;
       break;
     case BLE_SWITCH_ON:
-      memcpy(drv->adv_name, command->data.name, sizeof(drv->adv_name));
+      memcpy(&drv->adv_cmd, &command->data.adv_start, sizeof(drv->adv_cmd));
       drv->mode_requested = BLE_MODE_CONNECTABLE;
       break;
     case BLE_PAIRING_MODE:
-      memcpy(drv->adv_name, command->data.name, sizeof(drv->adv_name));
+      memcpy(&drv->adv_cmd, &command->data.adv_start, sizeof(drv->adv_cmd));
       drv->mode_requested = BLE_MODE_PAIRING;
       break;
     case BLE_DISCONNECT:
@@ -585,8 +610,40 @@ void ble_get_state(ble_state_t *state) {
   state->pairing = drv->mode_current == BLE_MODE_PAIRING;
   state->connectable = drv->mode_current == BLE_MODE_CONNECTABLE;
   state->pairing_requested = drv->pairing_requested;
+  state->state_known = drv->status_valid;
 
   irq_unlock(key);
 }
 
+bool ble_get_mac(uint8_t *mac, size_t max_len) {
+  ble_driver_t *drv = &g_ble_driver;
+
+  if (max_len < sizeof(drv->mac)) {
+    return false;
+  }
+
+  if (!drv->initialized) {
+    memset(mac, 0, max_len);
+    return false;
+  }
+
+  drv->mac_ready = false;
+
+  if (!ble_send_mac_request(drv)) {
+    return false;
+  }
+
+  uint32_t timeout = ticks_timeout(100);
+
+  while (!ticks_expired(timeout)) {
+    if (drv->mac_ready) {
+      memcpy(mac, drv->mac, sizeof(drv->mac));
+      return true;
+    }
+  }
+
+  memset(mac, 0, max_len);
+  return false;
+}
+
 #endif

core/embed/io/ble/stm32/ble_comm_defs.h

@@ -45,6 +45,7 @@ typedef enum {
   INTERNAL_EVENT_STATUS = 0x01,
   INTERNAL_EVENT_PAIRING_REQUEST = 0x04,
   INTERNAL_EVENT_PAIRING_CANCELLED = 0x05,
+  INTERNAL_EVENT_MAC = 0x06,
 } internal_event_t;
 
 typedef enum {
@@ -56,11 +57,14 @@ typedef enum {
   INTERNAL_CMD_ACK = 0x05,
   INTERNAL_CMD_ALLOW_PAIRING = 0x06,
   INTERNAL_CMD_REJECT_PAIRING = 0x07,
+  INTERNAL_CMD_MAC_REQUEST = 0x09,
 } internal_cmd_t;
 
 typedef struct {
   uint8_t cmd_id;
   uint8_t whitelist;
   uint8_t color;
+  uint8_t static_addr;
+  uint32_t device_code;
   uint8_t name[BLE_ADV_NAME_LEN];
 } cmd_advertising_on_t;

core/embed/projects/prodtest/README.md

@@ -130,6 +130,54 @@ bootloader-version
 OK 2.1.7
 ```
 
+### ble-adv-start
+Starts BLE advertising. Accepts one parameter, advertising name. The command returns `OK` if the operation is successful.
+
+`ble-adv-start <name>`
+
+Example:
+```
+ble-adv-start TREZOR_BLE
+# Advertising started.
+OK
+```
+
+### ble-adv-stop
+Stops BLE advertising. The command returns `OK` if the operation is successful.
+
+`ble-adv-stop`
+
+Example:
+```
+ble-adv-stop
+# Advertising stopped.
+OK
+```
+
+### ble-info
+Reads info about BLE. Currently, it returns only MAC address.
+The advertising needs to be started before the MAC can be read,
+otherwise the address will be random and will not correspond to actual MAC used.
+
+`ble-info`
+
+Example:
+```
+ble-info
+# MAC: 56:0b:b8:99:32:23
+OK
+```
+
+### ble-erase-bonds
+Erases all BLE bonds from the device.
+
+Example:
+```
+ble-erase-bonds
+# Erased 2 bonds.
+OK
+```
+
 ### button-test
 The `button-test` command tests the functionality of the device's hardware buttons. It waits for the user to press and release a specified button in a designated timeout period.
 

core/embed/projects/prodtest/cmd/prodtest_ble.c

@@ -0,0 +1,258 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifdef USE_BLE
+
+#include <trezor_rtl.h>
+
+#include <io/ble.h>
+#include <rtl/cli.h>
+#include <sys/systick.h>
+#include <sys/systimer.h>
+
+void ble_timer_cb(void* context) {
+  ble_event_t e = {0};
+  ble_command_t cmd = {0};
+
+  bool event_received = ble_get_event(&e);
+
+  if (event_received) {
+    switch (e.type) {
+      case BLE_PAIRING_REQUEST:
+        cmd.cmd_type = BLE_ALLOW_PAIRING;
+        ble_issue_command(&cmd);
+      default:
+        break;
+    }
+  }
+}
+
+static bool ensure_ble_init(cli_t* cli) {
+  cli_trace(cli, "Initializing the BLE...");
+  if (!ble_init()) {
+    cli_error(cli, CLI_ERROR, "Cannot initialize BLE.");
+    return false;
+  }
+
+  static systimer_t* timer = NULL;
+
+  if (timer == NULL) {
+    timer = systimer_create(ble_timer_cb, NULL);
+    if (timer == NULL) {
+      cli_error(cli, CLI_ERROR, "Cannot create timer.");
+      return false;
+    }
+    systimer_set_periodic(timer, 10);
+  }
+
+  return true;
+}
+
+static void prodtest_ble_adv_start(cli_t* cli) {
+  const char* name = cli_arg(cli, "name");
+
+  if (cli_arg_count(cli) > 1) {
+    cli_error_arg_count(cli);
+    return;
+  }
+
+  if (!ensure_ble_init(cli)) {
+    return;
+  }
+
+  uint16_t name_len =
+      strlen(name) > BLE_ADV_NAME_LEN ? BLE_ADV_NAME_LEN : strlen(name);
+
+  ble_command_t cmd = {0};
+  cmd.cmd_type = BLE_PAIRING_MODE;
+  cmd.data_len = sizeof(cmd.data.adv_start);
+  cmd.data.adv_start.static_mac = true;
+  memcpy(cmd.data.adv_start.name, name, name_len);
+
+  if (!ble_issue_command(&cmd)) {
+    cli_error(cli, CLI_ERROR, "Could not start advertising.");
+    return;
+  }
+
+  uint32_t timeout = ticks_timeout(1000);
+
+  bool result = false;
+  while (!ticks_expired(timeout)) {
+    ble_state_t state = {0};
+    ble_get_state(&state);
+
+    if (state.pairing) {
+      result = true;
+      break;
+    }
+  }
+
+  if (!result) {
+    cli_error(cli, CLI_ERROR, "Could not start advertising.");
+    return;
+  }
+
+  cli_trace(cli, "Advertising started.");
+  cli_ok(cli, "");
+}
+
+static void prodtest_ble_adv_stop(cli_t* cli) {
+  if (cli_arg_count(cli) > 0) {
+    cli_error_arg_count(cli);
+    return;
+  }
+
+  if (!ensure_ble_init(cli)) {
+    return;
+  }
+
+  ble_command_t cmd = {0};
+  cmd.cmd_type = BLE_SWITCH_OFF;
+  cmd.data_len = 0;
+
+  if (!ble_issue_command(&cmd)) {
+    cli_error(cli, CLI_ERROR, "Could not stop advertising.");
+    return;
+  }
+
+  uint32_t timeout = ticks_timeout(1000);
+
+  bool result = false;
+  while (!ticks_expired(timeout)) {
+    ble_state_t state = {0};
+    ble_get_state(&state);
+
+    if (!state.pairing && !state.connectable) {
+      result = true;
+      break;
+    }
+  }
+
+  if (!result) {
+    cli_error(cli, CLI_ERROR, "Could not stop advertising.");
+    return;
+  }
+
+  cli_trace(cli, "Advertising stopped.");
+  cli_ok(cli, "");
+}
+
+static void prodtest_ble_info(cli_t* cli) {
+  if (cli_arg_count(cli) > 0) {
+    cli_error_arg_count(cli);
+    return;
+  }
+
+  if (!ensure_ble_init(cli)) {
+    return;
+  }
+
+  uint8_t mac[6] = {0};
+
+  if (!ble_get_mac(mac, 6)) {
+    cli_error(cli, CLI_ERROR, "Could not read MAC.");
+    return;
+  }
+
+  cli_trace(cli, "MAC: %02x:%02x:%02x:%02x:%02x:%02x", mac[5], mac[4], mac[3],
+            mac[2], mac[1], mac[0]);
+  cli_ok(cli, "");
+}
+
+bool prodtest_ble_erase_bonds(cli_t* cli) {
+  ble_command_t cmd = {0};
+  cmd.cmd_type = BLE_ERASE_BONDS;
+
+  ble_state_t state = {0};
+  ble_issue_command(&cmd);
+
+  uint32_t timeout = ticks_timeout(100);
+
+  while (!ticks_expired(timeout)) {
+    ble_get_state(&state);
+    if (state.peer_count == 0 && state.state_known) {
+      return true;
+    }
+  }
+
+  return false;
+}
+
+static void prodtest_ble_erase_bonds_cmd(cli_t* cli) {
+  if (cli_arg_count(cli) > 0) {
+    cli_error_arg_count(cli);
+    return;
+  }
+
+  if (!ensure_ble_init(cli)) {
+    return;
+  }
+
+  ble_state_t state = {0};
+
+  ble_get_state(&state);
+
+  if (!state.state_known) {
+    cli_error(cli, CLI_ERROR, "BLE state unknown.");
+  }
+
+  if (state.peer_count == 0) {
+    cli_ok(cli, "No bonds to erase.");
+    return;
+  }
+
+  if (!prodtest_ble_erase_bonds(cli)) {
+    cli_error(cli, CLI_ERROR, "Could not erase bonds.");
+  }
+
+  cli_trace(cli, "Erased %d bonds.", state.peer_count);
+  cli_ok(cli, "");
+}
+
+// clang-format off
+
+PRODTEST_CLI_CMD(
+  .name = "ble-adv-start",
+  .func = prodtest_ble_adv_start,
+  .info = "Start BLE advertising",
+  .args = "<name>"
+);
+
+PRODTEST_CLI_CMD(
+  .name = "ble-adv-stop",
+  .func = prodtest_ble_adv_stop,
+  .info = "Stop BLE advertising",
+  .args = "<name>"
+);
+
+PRODTEST_CLI_CMD(
+  .name = "ble-info",
+  .func = prodtest_ble_info,
+  .info = "Get BLE information",
+  .args = ""
+);
+
+PRODTEST_CLI_CMD(
+  .name = "ble-erase-bonds",
+  .func = prodtest_ble_erase_bonds_cmd,
+  .info = "Erase all BLE bonds",
+  .args = ""
+);
+
+#endif

core/embed/projects/prodtest/cmd/prodtest_ble.h

@@ -0,0 +1,26 @@
+/*
+ * This file is part of the Trezor project, https://trezor.io/
+ *
+ * Copyright (c) SatoshiLabs
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <trezor_types.h>
+
+#include <rtl/cli.h>
+
+bool prodtest_ble_erase_bonds(cli_t* cli);

core/embed/projects/prodtest/cmd/prodtest_prodtest.c

@@ -24,6 +24,10 @@
 #include <rtl/cli.h>
 #include <util/fwutils.h>
 
+#ifdef USE_BLE
+#include "prodtest_ble.h"
+#endif
+
 #include <version.h>
 
 static void prodtest_prodtest_intro(cli_t* cli) {
@@ -49,6 +53,14 @@ static void prodtest_prodtest_wipe(cli_t* cli) {
     return;
   }
 
+#ifdef USE_BLE
+  cli_trace(cli, "Erasing BLE bonds...");
+  if (!prodtest_ble_erase_bonds(cli)) {
+    cli_error(cli, CLI_ERROR, "Failed to erase BLE bonds.");
+    return;
+  }
+#endif
+
   cli_trace(cli, "Invalidating the production test firmware header...");
   firmware_invalidate_header();
 

core/embed/projects/prodtest/main.c

@@ -26,6 +26,7 @@
 #include <sys/system.h>
 #include <util/flash_otp.h>
 #include <util/rsod.h>
+#include <util/unit_properties.h>
 
 #include "rust_ui_prodtest.h"
 
@@ -79,6 +80,12 @@
 #include <sec/secure_aes.h>
 #endif
 
+#ifdef USE_BLE
+#include <io/ble.h>
+#include <util/unit_properties.h>
+#include "cmd/prodtest_ble.h"
+#endif
+
 #ifdef TREZOR_MODEL_T2T1
 #define MODEL_IDENTIFIER "TREZOR2-"
 #else
@@ -193,6 +200,10 @@ static void drivers_init(void) {
 #ifdef USE_RGB_LED
   rgb_led_init();
 #endif
+#ifdef USE_BLE
+  unit_properties_init();
+  ble_init();
+#endif
 #ifdef USE_TROPIC
   tropic_init();
 #endif

core/embed/rust/src/trezorhal/ble.rs

@@ -8,6 +8,7 @@ pub fn connected() -> bool {
             connectable: false,
             pairing: false,
             pairing_requested: false,
+            state_known: false,
         };
         ffi::ble_get_state(&mut state as _);
 
@@ -26,9 +27,9 @@ pub fn pairing_mode(name: &str) {
         let bytes = name.as_bytes();
 
         // Determine how many bytes we can copy (min of buffer size and string length).
-        let len = bytes.len().min(cmd.data.name.len());
+        let len = bytes.len().min(cmd.data.adv_start.name.len());
 
-        cmd.data.name[..len].copy_from_slice(&bytes[..len]);
+        cmd.data.adv_start.name[..len].copy_from_slice(&bytes[..len]);
 
         ffi::ble_issue_command(&mut cmd as _);
     }

core/embed/upymod/modtrezorio/modtrezorio-ble.h

@@ -173,12 +173,13 @@ STATIC mp_obj_t mod_trezorio_BLE_start_advertising(size_t n_args,
 
   ble_command_t cmd = {
       .cmd_type = whitelist_bool ? BLE_SWITCH_ON : BLE_PAIRING_MODE,
-      .data_len = name.len};
+      .data_len = sizeof(ble_adv_start_cmd_data_t)};
 
   // get a minimum of the two lengths
   int len = name_len < BLE_ADV_NAME_LEN ? name_len : BLE_ADV_NAME_LEN;
 
-  memcpy(cmd.data.name, name_buf, len);
+  cmd.data.adv_start.static_mac = false;
+  memcpy(cmd.data.adv_start.name, name_buf, len);
 
   return mp_obj_new_bool(ble_issue_command(&cmd));
 }

nordic/trezor/trezor-ble/src/ble/advertising.c

@@ -35,7 +35,7 @@ LOG_MODULE_REGISTER(LOG_MODULE_NAME);
 bool advertising = false;
 bool advertising_wl = false;
 
-uint8_t manufacturer_data[8] = {0xff, 0xff, 0, 0, 'T', '3', 'W', '1'};
+uint8_t manufacturer_data[8] = {0xff, 0xff, 0, 0, 0, 0, 0, 0};
 
 static struct bt_data advertising_data[2];
 
@@ -61,7 +61,8 @@ void advertising_setup_wl(void) {
   bt_foreach_bond(BT_ID_DEFAULT, add_to_whitelist, NULL);
 }
 
-void advertising_start(bool wl, uint8_t color, char *name, int name_len) {
+void advertising_start(bool wl, uint8_t color, uint32_t device_code,
+                       bool static_addr, char *name, int name_len) {
   if (advertising) {
     LOG_WRN("Restarting advertising");
     bt_le_adv_stop();
@@ -74,6 +75,10 @@ void advertising_start(bool wl, uint8_t color, char *name, int name_len) {
   }
 
   manufacturer_data[3] = color;
+  manufacturer_data[4] = (device_code >> 24) & 0xff;
+  manufacturer_data[5] = (device_code >> 16) & 0xff;
+  manufacturer_data[6] = (device_code >> 8) & 0xff;
+  manufacturer_data[7] = device_code & 0xff;
 
   advertising_data[0].type = BT_DATA_FLAGS;
   advertising_data[0].data_len = 1;
@@ -96,23 +101,31 @@ void advertising_start(bool wl, uint8_t color, char *name, int name_len) {
 
     manufacturer_data[2] = 0x00;
 
-    err = bt_le_adv_start(
-        BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_SCANNABLE |
-                            BT_LE_ADV_OPT_FILTER_CONN |
-                            BT_LE_ADV_OPT_FILTER_SCAN_REQ,
-                        160, 1600, NULL),
-        advertising_data, ARRAY_SIZE(advertising_data), scan_response_data,
-        ARRAY_SIZE(scan_response_data));
+    uint32_t options = BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_SCANNABLE |
+                       BT_LE_ADV_OPT_FILTER_CONN |
+                       BT_LE_ADV_OPT_FILTER_SCAN_REQ;
+    if (static_addr) {
+      LOG_ERR("Advertising with static ADDR");
+      options |= BT_LE_ADV_OPT_USE_IDENTITY;
+    }
+
+    err = bt_le_adv_start(BT_LE_ADV_PARAM(options, 160, 1600, NULL),
+                          advertising_data, ARRAY_SIZE(advertising_data),
+                          scan_response_data, ARRAY_SIZE(scan_response_data));
   } else {
     LOG_INF("Advertising no whitelist");
 
     manufacturer_data[2] = 0x01;
 
-    err = bt_le_adv_start(
-        BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_SCANNABLE,
-                        160, 1600, NULL),
-        advertising_data, ARRAY_SIZE(advertising_data), scan_response_data,
-        ARRAY_SIZE(scan_response_data));
+    uint32_t options = BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_SCANNABLE;
+    if (static_addr) {
+      LOG_ERR("Advertising with static ADDR");
+      options |= BT_LE_ADV_OPT_USE_IDENTITY;
+    }
+
+    err = bt_le_adv_start(BT_LE_ADV_PARAM(options, 160, 1600, NULL),
+                          advertising_data, ARRAY_SIZE(advertising_data),
+                          scan_response_data, ARRAY_SIZE(scan_response_data));
   }
   if (err) {
     LOG_ERR("Advertising failed to start (err %d)", err);
@@ -152,3 +165,28 @@ void advertising_init(void) {
   LOG_INF("Advertising init");
   advertising_setup_wl();
 }
+
+void advertising_get_mac(uint8_t *mac, uint16_t max_len) {
+  bt_addr_le_t addr[CONFIG_BT_ID_MAX] = {0};
+  size_t count = 0;
+
+  // Get the first (default) identity address
+  bt_id_get(addr, &count);
+
+  struct bt_le_oob oob_data;
+  bt_le_oob_get_local(BT_ID_DEFAULT, &oob_data);
+
+  for (size_t i = 0; i < count; i++) {
+    char addr_str[BT_ADDR_LE_STR_LEN];
+    bt_addr_le_to_str(addr, addr_str, sizeof(addr_str));
+    LOG_ERR("Current BT MAC Address: %s\n", addr_str);
+  }
+
+  char addr_str[BT_ADDR_LE_STR_LEN];
+  bt_addr_le_to_str(&oob_data.addr, addr_str, sizeof(addr_str));
+  LOG_ERR("Current BT MAC Address: %s\n", addr_str);
+
+  LOG_ERR("Num of IDS: %d", count);
+
+  memcpy(mac, oob_data.addr.a.val, max_len);
+}

nordic/trezor/trezor-ble/src/ble/ble_internal.h

@@ -70,6 +70,7 @@ typedef enum {
   INTERNAL_EVENT_FAILURE = 0x03,
   INTERNAL_EVENT_PAIRING_REQUEST = 0x04,
   INTERNAL_EVENT_PAIRING_CANCELLED = 0x05,
+  INTERNAL_EVENT_MAC = 0x06,
 } internal_event_t;
 
 typedef enum {
@@ -82,6 +83,7 @@ typedef enum {
   INTERNAL_CMD_ALLOW_PAIRING = 0x06,
   INTERNAL_CMD_REJECT_PAIRING = 0x07,
   INTERNAL_CMD_UNPAIR = 0x08,
+  INTERNAL_CMD_GET_MAC = 0x09,
 } internal_cmd_t;
 
 // BLE management functions
@@ -106,13 +108,16 @@ bool bonds_erase_current(void);
 // Initialization
 void advertising_init(void);
 // Start advertising, with or without whitelist
-void advertising_start(bool wl, uint8_t color, char *name, int name_len);
+void advertising_start(bool wl, uint8_t color, uint32_t device_code,
+                       bool static_addr, char *name, int name_len);
 // Stop advertising
 void advertising_stop(void);
 // Check if advertising is active
 bool advertising_is_advertising(void);
 // Check if advertising is active with whitelist
 bool advertising_is_advertising_whitelist(void);
+// Get current MAC address
+void advertising_get_mac(uint8_t *mac, uint16_t max_len);
 
 // Connection functions
 // Initialization

nordic/trezor/trezor-ble/src/ble/management.c

@@ -96,6 +96,13 @@ void management_send_pairing_request_event(uint8_t *data, uint16_t len) {
   trz_comm_send_msg(NRF_SERVICE_BLE_MANAGER, tx_data, sizeof(tx_data));
 }
 
+void management_send_mac(uint8_t *mac) {
+  uint8_t tx_data[1 + BT_ADDR_SIZE] = {0};
+  tx_data[0] = INTERNAL_EVENT_MAC;
+  memcpy(&tx_data[1], mac, BT_ADDR_SIZE);
+  trz_comm_send_msg(NRF_SERVICE_BLE_MANAGER, tx_data, sizeof(tx_data));
+}
+
 static void process_command(uint8_t *data, uint16_t len) {
   uint8_t cmd = data[0];
   bool success = true;
@@ -107,9 +114,14 @@ static void process_command(uint8_t *data, uint16_t len) {
       break;
     case INTERNAL_CMD_ADVERTISING_ON: {
       uint8_t color = data[2];
-      char *name = &data[3];
+      bool static_addr = data[3];
+      uint32_t device_code =
+          (data[4] << 24) | (data[5] << 16) | (data[6] << 8) | data[7];
+      char *name = &data[8];
+
       int name_len = strnlen(name, 20);
-      advertising_start(data[1] != 0, color, name, name_len);
+      advertising_start(data[1] != 0, color, device_code, static_addr, name,
+                        name_len);
     } break;
     case INTERNAL_CMD_ADVERTISING_OFF:
       advertising_stop();
@@ -131,6 +143,12 @@ static void process_command(uint8_t *data, uint16_t len) {
     case INTERNAL_CMD_UNPAIR:
       success = bonds_erase_current();
       break;
+    case INTERNAL_CMD_GET_MAC: {
+      uint8_t mac[BT_ADDR_SIZE] = {0};
+      advertising_get_mac(mac, BT_ADDR_SIZE);
+      management_send_mac(mac);
+      send_response = false;
+    } break;
     default:
       break;
   }