Sequential dual-identity BLE on nRF52832 / SoftDevice s132 v6.1.1: one chip, two MAC addresses, mid-connection GAP identity switch so a Garmin watch pairs the HRM while a Connect IQ datafield uses a custom service. Reference C++.

ble-identity-switch.cpp

// ble-identity-switch.cpp
//
// Sequential dual-identity BLE on a legacy Nordic SoftDevice (s132 v6.1.1 /
// nRF52832). One physical peripheral presents as TWO devices at two MAC
// addresses, switching its GAP identity mid-connection so a Garmin watch can
// pair it natively as a heart-rate monitor (MAC_A) while a Connect IQ datafield
// connects to a custom service (MAC_B) without tripping Garmin's "two
// connections to one peripheral" refusal.
//
// This is a stripped-down, vendor-neutral reference: the production version runs
// on Movesense (Whiteboard event API), but the logic that matters is here. It
// is NOT drop-in compilable: it needs the SoftDevice and your stack's BLE event
// plumbing. CC0 / public domain. Take it and go.

#include <cstdint>

// ───────────────────────────────────────────────────────────────────────────
// 1. SoftDevice SVC trampolines
//
// Closed BLE binaries (e.g. MovesenseCoreLib) don't export Nordic's generated
// sd_* stubs, so we emit the SVC instruction directly. SVC numbers for s132
// v6.1.1 come from BLE_GAP_SVC_BASE = 0x6C plus the enum offset:
//   addr_set = 0x6C, addr_get = 0x6D, adv_stop = 0x74
// AAPCS puts the first argument in r0, which is exactly where the SoftDevice's
// SVC handler expects the pointer; bx lr returns the result code it left in r0.
// ───────────────────────────────────────────────────────────────────────────

struct __attribute__((packed)) gap_addr_t {
    uint8_t flags;    // bit 0 = addr_id_peer, bits 1-7 = addr_type
    uint8_t addr[6];  // little-endian MAC
};

__attribute__((naked, noinline))
static uint32_t sd_gap_addr_set(gap_addr_t const* /*p_addr*/) {
    __asm volatile("svc 0x6C\n" "bx lr");
}
__attribute__((naked, noinline))
static uint32_t sd_gap_addr_get(gap_addr_t* /*p_addr*/) {
    __asm volatile("svc 0x6D\n" "bx lr");
}
__attribute__((naked, noinline))
static uint32_t sd_gap_adv_stop(uint8_t /*adv_handle*/) {
    __asm volatile("svc 0x74\n" "bx lr");
}

static constexpr uint32_t NRF_SUCCESS             = 0x00;
static constexpr uint32_t NRF_ERROR_INVALID_STATE = 0x08;

// ───────────────────────────────────────────────────────────────────────────
// 2. Platform hooks — implement these against your BLE stack
// ───────────────────────────────────────────────────────────────────────────
extern void advertise_hrs();           // adv packet: HRS 0x180D only
extern void advertise_custom();        // adv packet: F0F0 + 128-bit UUID, no HRS
extern void start_timer(uint32_t ms);  // one-shot; fires on_timer()
extern void debug_log(const char* msg);

static void nvic_system_reset();       // defined at the bottom

static constexpr uint32_t SWITCH_DELAY_MS   = 5000;  // CONNECTED -> switch
static constexpr uint32_t RECOVERY_GRACE_MS = 5000;  // last peer gone -> reboot

// ───────────────────────────────────────────────────────────────────────────
// 3. State
// ───────────────────────────────────────────────────────────────────────────
enum class State { PRE_SWITCH, SCHEDULED, POST_SWITCH };

static State    g_state             = State::PRE_SWITCH;
static int      g_peers             = 0;
static bool     g_first_peer_valid  = false;
static uint16_t g_first_peer_handle = 0;  // the HRS (watch) connection on MAC_A

// MAC_B is derived from the factory MAC by flipping one byte: deterministic,
// re-created on every boot (addr_set is RAM-only), and guaranteed != MAC_A.
static gap_addr_t mac_b_from_factory() {
    gap_addr_t a{};
    sd_gap_addr_get(&a);
    a.addr[0] ^= 0xAA;
    return a;
}

// ───────────────────────────────────────────────────────────────────────────
// 4. The switch — runs SWITCH_DELAY_MS after the first CONNECTED
// ───────────────────────────────────────────────────────────────────────────
static void do_switch() {
    // adv_stop usually returns INVALID_STATE here: the SoftDevice already
    // stopped advertising when slot 1 filled on CONNECTED. Expected, ignore it.
    (void)sd_gap_adv_stop(0);

    gap_addr_t mac_b = mac_b_from_factory();
    uint32_t err = sd_gap_addr_set(&mac_b);  // returns NRF_SUCCESS mid-connection
    if (err != NRF_SUCCESS) { debug_log("addr_set failed"); return; }

    // The load-bearing fact: the existing MAC_A connection SURVIVES this. Same
    // conn_handle, CCCD, MTU, HR notifications still flowing. The watch never
    // notices its peer's GAP identity changed underneath it.
    advertise_custom();                      // now advertising as MAC_B
    g_state = State::POST_SWITCH;
}

// ───────────────────────────────────────────────────────────────────────────
// 5. Event handlers — wire these to your stack's BLE events
// ───────────────────────────────────────────────────────────────────────────
void on_boot() {
    g_state = State::PRE_SWITCH;
    advertise_hrs();                         // advertise as MAC_A (factory MAC)
}

void on_connected(uint16_t conn_handle) {
    g_peers++;
    if (g_state == State::PRE_SWITCH) {
        // Pre-switch only HRS is advertised, so the first peer is the watch.
        g_first_peer_handle = conn_handle;
        g_first_peer_valid  = true;
        g_state = State::SCHEDULED;
        start_timer(SWITCH_DELAY_MS);        // let MTU + HRS CCCD settle first
    }
}

void on_timer() {
    if (g_state == State::SCHEDULED) {
        if (g_peers == 0) {                  // HR dropped during the delay:
            g_state = State::PRE_SWITCH;     // abort the switch, stay on MAC_A
            advertise_hrs();
            return;
        }
        do_switch();
    } else if (g_state == State::POST_SWITCH && g_peers == 0) {
        nvic_system_reset();                 // recovery grace expired, no peers
    }
}

void on_disconnected(uint16_t conn_handle) {
    g_peers--;
    const bool dropper_is_hrs =
        g_first_peer_valid && conn_handle == g_first_peer_handle;

    if (g_peers == 0) {
        // Everyone gone. The clean revert (addr_set back to MAC_A by hand)
        // wedges on the 2nd switch: the adv-PUT pipeline desyncs and adv_start
        // silently stops putting anything on the air. So we reboot instead:
        // back up on the factory MAC in ~1.5s, bonded HR auto-reconnects.
        start_timer(RECOVERY_GRACE_MS);      // grace lets a fast reconnect win
        return;
    }

    if (g_state == State::POST_SWITCH && !dropper_is_hrs) {
        // Datafield dropped, HR still connected: re-advertise MAC_B in place so
        // the datafield can find us again. No address change (that would kill
        // the surviving HR link).
        debug_log("datafield dropped, HR remains -- re-advertising MAC_B");
        advertise_custom();
    }
    // HR dropped, datafield still connected: do nothing on purpose. Staying on
    // MAC_B keeps the datafield alive; HR re-pair waits for the next reboot.
    // An immediate addr_set back to MAC_A here kills the datafield mid-setup.
}

// ───────────────────────────────────────────────────────────────────────────
// 6. NVIC system reset, inlined (CMSIS headers aren't on the app include path)
//    SCB_AIRCR @ 0xE000ED0C: VECTKEY (0x5FA, high half) + SYSRESETREQ (bit 2).
// ───────────────────────────────────────────────────────────────────────────
static void nvic_system_reset() {
    __asm__ volatile("dsb 0xF" ::: "memory");
    *(volatile uint32_t*)0xE000ED0Cu = 0x05FA0004u;
    __asm__ volatile("dsb 0xF" ::: "memory");
    for (;;) { __asm__ volatile("nop"); }
}