NimBLE-Arduino testcase for saving bonding info for multiple devices.

nimble_test.cpp

/** NimBLE_Server Demo:
 *
 *  Demonstrates many of the available features of the NimBLE server library.
 *
 *  Created: on March 22 2020
 *      Author: H2zero
 * 
 * Modified: on November 30 2023
 *      Author: bblodget
 * 
 *      Modified to use the NimBLEHIDDevice class to create a minimal HID Device.
 *      Testing connecting with up to 3 clients and seeing if bonding information
 *     is stored and retrieved from NVS correctly. 
 *
 */

#include <Arduino.h>
#include <ArduinoLog.h>
#include <NimBLEDevice.h>
#include <NimBLEHIDDevice.h>


#define LOG_TAG ""

// Report IDs:
#define KEYBOARD_ID 0x01
#define MEDIA_KEYS_ID 0x02

static const uint8_t _hidReportDescriptor[] = {
    USAGE_PAGE(1), 0x01, // USAGE_PAGE (Generic Desktop Ctrls)
    USAGE(1), 0x06,      // USAGE (Keyboard)
    COLLECTION(1), 0x01, // COLLECTION (Application)
    // ------------------------------------------------- Keyboard
    REPORT_ID(1), KEYBOARD_ID, //   REPORT_ID (1)
    USAGE_PAGE(1), 0x07,       //   USAGE_PAGE (Kbrd/Keypad)
    USAGE_MINIMUM(1), 0xE0,    //   USAGE_MINIMUM (0xE0)
    USAGE_MAXIMUM(1), 0xE7,    //   USAGE_MAXIMUM (0xE7)
    LOGICAL_MINIMUM(1), 0x00,  //   LOGICAL_MINIMUM (0)
    LOGICAL_MAXIMUM(1), 0x01,  //   Logical Maximum (1)
    REPORT_SIZE(1), 0x01,      //   REPORT_SIZE (1)
    REPORT_COUNT(1), 0x08,     //   REPORT_COUNT (8)
    HIDINPUT(1), 0x02,         //   INPUT (Data,Var,Abs,No Wrap,Linear,Preferred
                               //   State,No Null Position)
    REPORT_COUNT(1), 0x01,     //   REPORT_COUNT (1) ; 1 byte (Reserved)
    REPORT_SIZE(1), 0x08,      //   REPORT_SIZE (8)
    HIDINPUT(1), 0x01,      //   INPUT (Const,Array,Abs,No Wrap,Linear,Preferred
                            //   State,No Null Position)
    REPORT_COUNT(1), 0x05,  //   REPORT_COUNT (5) ; 5 bits (Num lock, Caps lock,
                            //   Scroll lock, Compose, Kana)
    REPORT_SIZE(1), 0x01,   //   REPORT_SIZE (1)
    USAGE_PAGE(1), 0x08,    //   USAGE_PAGE (LEDs)
    USAGE_MINIMUM(1), 0x01, //   USAGE_MINIMUM (0x01) ; Num Lock
    USAGE_MAXIMUM(1), 0x05, //   USAGE_MAXIMUM (0x05) ; Kana
    HIDOUTPUT(1), 0x02,     //   OUTPUT (Data,Var,Abs,No Wrap,Linear,Preferred
                            //   State,No Null Position,Non-volatile)
    REPORT_COUNT(1), 0x01,  //   REPORT_COUNT (1) ; 3 bits (Padding)
    REPORT_SIZE(1), 0x03,   //   REPORT_SIZE (3)
    HIDOUTPUT(1), 0x01,    //   OUTPUT (Const,Array,Abs,No Wrap,Linear,Preferred
                           //   State,No Null Position,Non-volatile)
    REPORT_COUNT(1), 0x06, //   REPORT_COUNT (6) ; 6 bytes (Keys)
    REPORT_SIZE(1), 0x08,  //   REPORT_SIZE(8)
    LOGICAL_MINIMUM(1), 0x00, //   LOGICAL_MINIMUM(0)
    LOGICAL_MAXIMUM(1), 0x65, //   LOGICAL_MAXIMUM(0x65) ; 101 keys
    USAGE_PAGE(1), 0x07,      //   USAGE_PAGE (Kbrd/Keypad)
    USAGE_MINIMUM(1), 0x00,   //   USAGE_MINIMUM (0)
    USAGE_MAXIMUM(1), 0x65,   //   USAGE_MAXIMUM (0x65)
    HIDINPUT(1), 0x00, //   INPUT (Data,Array,Abs,No Wrap,Linear,Preferred
                       //   State,No Null Position)
    END_COLLECTION(0), // END_COLLECTION
    // ------------------------------------------------- Media Keys
    USAGE_PAGE(1), 0x0C,         // USAGE_PAGE (Consumer)
    USAGE(1), 0x01,              // USAGE (Consumer Control)
    COLLECTION(1), 0x01,         // COLLECTION (Application)
    REPORT_ID(1), MEDIA_KEYS_ID, //   REPORT_ID (3)
    USAGE_PAGE(1), 0x0C,         //   USAGE_PAGE (Consumer)
    LOGICAL_MINIMUM(1), 0x00,    //   LOGICAL_MINIMUM (0)
    LOGICAL_MAXIMUM(1), 0x01,    //   LOGICAL_MAXIMUM (1)
    REPORT_SIZE(1), 0x01,        //   REPORT_SIZE (1)
    REPORT_COUNT(1), 0x10,       //   REPORT_COUNT (16)
    USAGE(1), 0xB5,              //   USAGE (Scan Next Track)     ; bit 0: 1
    USAGE(1), 0xB6,              //   USAGE (Scan Previous Track) ; bit 1: 2
    USAGE(1), 0xB7,              //   USAGE (Stop)                ; bit 2: 4
    USAGE(1), 0xCD,              //   USAGE (Play/Pause)          ; bit 3: 8
    USAGE(1), 0xE2,              //   USAGE (Mute)                ; bit 4: 16
    USAGE(1), 0xE9,              //   USAGE (Volume Increment)    ; bit 5: 32
    USAGE(1), 0xEA,              //   USAGE (Volume Decrement)    ; bit 6: 64
    USAGE(2), 0x23, 0x02,        //   Usage (WWW Home)            ; bit 7: 128
    USAGE(2), 0x94, 0x01,        //   Usage (My Computer) ; bit 0: 1
    USAGE(2), 0x92, 0x01,        //   Usage (Calculator)  ; bit 1: 2
    USAGE(2), 0x2A, 0x02,        //   Usage (WWW fav)     ; bit 2: 4
    USAGE(2), 0x21, 0x02,        //   Usage (WWW search)  ; bit 3: 8
    USAGE(2), 0x26, 0x02,        //   Usage (WWW stop)    ; bit 4: 16
    USAGE(2), 0x24, 0x02,        //   Usage (WWW back)    ; bit 5: 32
    USAGE(2), 0x83, 0x01,        //   Usage (Media sel)   ; bit 6: 64
    USAGE(2), 0x8A, 0x01,        //   Usage (Mail)        ; bit 7: 128
    HIDINPUT(1), 0x02, //   INPUT (Data,Var,Abs,No Wrap,Linear,Preferred
                       //   State,No Null Position)
    END_COLLECTION(0)  // END_COLLECTION
};

static NimBLEServer*    pServer;
static NimBLEHIDDevice* pHid;

static BLECharacteristic* inputKeyboard;
static BLECharacteristic* outputKeyboard;
static BLECharacteristic* inputMediaKeys;

static uint16_t vid     = 0x05ac;
static uint16_t pid     = 0x820a;
static uint16_t version = 0x0210;

static auto LOG_LEVEL = LOG_LEVEL_NOTICE;

/**  None of these are required as they will be handled by the library with
 *defaults. **
 **                       Remove as you see fit for your needs */
class ServerCallbacks : public NimBLEServerCallbacks
{
public:
    std::string maskIPAddress(const std::string& macAddress) {
        // Assuming the MAC address is in the correct format
        if(macAddress.length() != 17) {
            // Handle error or return original string
            return macAddress; 
        }

        // Mask the first 5 bytes
        std::string maskedAddress = "xx:xx:xx:xx:xx:";

        // Append the last byte from the original MAC address
        maskedAddress += macAddress.substr(15);

        return maskedAddress;
    }

    String maskIPAddress(const ble_addr_t* addr) {
        String maskedAddress = "";

        for (int i = 0; i < 6; i++) {
            if (i < 5) {
                maskedAddress += "xx";
            } else {
                // Convert the last byte to a hexadecimal string
                if (addr->val[i] < 16) {
                    maskedAddress += "0"; // Add leading zero for single digit hex values
                }
                maskedAddress += String(addr->val[i], HEX);
            }

            if (i < 5) {
                maskedAddress += ":"; // Add colon separator except for the last byte
            }
        }

        return maskedAddress;
    }

    // Function to print BLE address
    void print_ble_addr(const ble_addr_t* addr)
    {
        NimBLEAddress n_addr = NimBLEAddress(*addr);
        String maskedAddress = maskIPAddress(addr);

        // Log.noticeln(LOG_TAG "%s", n_addr.toString().c_str());
        Log.noticeln(LOG_TAG "%s", maskedAddress.c_str());
    }
    

    // Function to pretty print the ble_gap_conn_desc structure
    void print_ble_gap_conn_desc(const struct ble_gap_conn_desc* desc)
    {
        Log.noticeln("BLE Connection Description:");
        Log.noticeln("Connection Handle: %d", desc->conn_handle);
        Log.noticeln("Connection Role: %s",
            desc->role == BLE_GAP_ROLE_SLAVE ? "Slave" : "Master");
        Log.noticeln("Connection Interval: %d", desc->conn_itvl);
        Log.noticeln("Connection Latency: %d", desc->conn_latency);
        Log.noticeln("Supervision Timeout: %d", desc->supervision_timeout);
        Log.noticeln("Master Clock Accuracy: %d", desc->master_clock_accuracy);

        Log.notice("Local Identity Address: ");
        print_ble_addr(&desc->our_id_addr);

        Log.notice("Peer Identity Address: ");
        print_ble_addr(&desc->peer_id_addr);

        Log.notice("Local OTA Address: ");
        print_ble_addr(&desc->our_ota_addr);

        Log.notice("Peer OTA Address: ");
        print_ble_addr(&desc->peer_ota_addr);
        Log.noticeln("");
    }

    void print_peer_info()
    {
        std::vector<uint16_t> deviceVector = pServer->getPeerDevices();
        for (uint8_t i = 0; i < deviceVector.size(); i++)
        {
            Log.noticeln("Device %d: %d", i, deviceVector[i]);
            // Log the peer info
            NimBLEConnInfo info = pServer->getPeerInfo(deviceVector[i]);
            Log.noticeln(
                "Peer Address: %s", maskIPAddress(info.getAddress().toString()).c_str());
            Log.noticeln("Peer Conn Handle: %d", info.getConnHandle());
            Log.noticeln("Peer isBonded: %d", info.isBonded() ? 1 : 0);
            Log.noticeln("Peer isEncrypted: %d", info.isEncrypted() ? 1 : 0);
            Log.noticeln(
                "Peer isAuthenticated: %d", info.isAuthenticated() ? 1 : 0);
        }

        Log.noticeln("%d devices connected:\n", deviceVector.size());
    }

    void print_bond_info()
    {
        // Print Bonding info
        int num_bonds = NimBLEDevice::getNumBonds();
        Log.noticeln("Number of bonds: %d", num_bonds);
        for (int i = 0; i < num_bonds; i++)
        {
            Log.noticeln("Bonded address(%d): %s", i,
                maskIPAddress(NimBLEDevice::getBondedAddress(i).toString()).c_str());
        }
    }

    void onConnect(NimBLEServer* pServer)
    {
        Serial.println("\nonConnect1...");
        Serial.println("Client connected");
        Serial.println("Multi-connect support: start advertising");
        NimBLEDevice::startAdvertising();
    };

    /** Alternative onConnect() method to extract details of the connection.
     *  See: src/ble_gap.h for the details of the ble_gap_conn_desc struct.
     */
    void onConnect(NimBLEServer* pServer, ble_gap_conn_desc* desc)
    {
        Serial.println("\nonConnect2...");
        Serial.print("Client address: ");
        Serial.println(maskIPAddress(NimBLEAddress(desc->peer_ota_addr).toString()).c_str());
        // Log the values of the deviceVector

        print_ble_gap_conn_desc(desc);
        print_peer_info();
        print_bond_info();

        /** We can use the connection handle here to ask for different
         * connection parameters. Args: connection handle, min connection
         * interval, max connection interval latency, supervision timeout.
         *  Units; Min/Max Intervals: 1.25 millisecond increments.
         *  Latency: number of intervals allowed to skip.
         *  Timeout: 10 millisecond increments, try for 5x interval time for
         * best results.
         */
        pServer->updateConnParams(desc->conn_handle, 24, 48, 0, 60);
    };
    void onDisconnect(NimBLEServer* pServer)
    {
        Serial.println("Client disconnected - start advertising");
        NimBLEDevice::startAdvertising();
    };
    void onMTUChange(uint16_t MTU, ble_gap_conn_desc* desc)
    {
        Serial.printf(
            "MTU updated: %u for connection ID: %u\n", MTU, desc->conn_handle);
    };

    /********************* Security handled here **********************
    ****** Note: these are the same return values as defaults ********/
    uint32_t onPassKeyRequest()
    {
        Serial.println("Server Passkey Request");
        /** This should return a random 6 digit number for security
         *  or make your own static passkey as done here.
         */
        return 123456;
    };

    bool onConfirmPIN(uint32_t pass_key)
    {
        Serial.print("The passkey YES/NO number: ");
        Serial.println(pass_key);
        /** Return false if passkeys don't match. */
        return true;
    };

    void onAuthenticationComplete(ble_gap_conn_desc* desc)
    {
        /** Check that encryption was successful, if not we disconnect the
         * client */
        if (!desc->sec_state.encrypted)
        {
            NimBLEDevice::getServer()->disconnect(desc->conn_handle);
            Serial.println("Encrypt connection failed - disconnecting client");
            return;
        }
        Serial.println("\nonAuthenticationComplete: success");
        print_peer_info();
        print_bond_info();
    };
};



static ServerCallbacks* pServerCallbacks;

void setup()
{
    Serial.begin(115200);
    Serial.println("Starting NimBLE Server");

    // Setup logging.
    bool showLevel = true;
    int  logLevel  = LOG_LEVEL;
    Log.begin(logLevel, &Serial, showLevel);

    /** sets device name */
    NimBLEDevice::init("NimBLE-Arduino");

    /** Optional: set the transmit power, default is 3db */
#ifdef ESP_PLATFORM
    NimBLEDevice::setPower(ESP_PWR_LVL_P9); /** +9db */
#else
    NimBLEDevice::setPower(9); /** +9db */
#endif

    /** Set the IO capabilities of the device, each option will trigger a
     * different pairing method. BLE_HS_IO_DISPLAY_ONLY    - Passkey pairing
     *  BLE_HS_IO_DISPLAY_YESNO   - Numeric comparison pairing
     *  BLE_HS_IO_NO_INPUT_OUTPUT - DEFAULT setting - just works pairing
     */
    // NimBLEDevice::setSecurityIOCap(BLE_HS_IO_DISPLAY_ONLY); // use passkey
    // NimBLEDevice::setSecurityIOCap(BLE_HS_IO_DISPLAY_YESNO); //use numeric
    // comparison

    /** 2 different ways to set security - both calls achieve the same result.
     *  no bonding, no man in the middle protection, secure connections.
     *
     *  These are the default values, only shown here for demonstration.
     */
    NimBLEDevice::setSecurityAuth(true, true, true);
    // NimBLEDevice::setSecurityAuth(/*BLE_SM_PAIR_AUTHREQ_BOND |
    // BLE_SM_PAIR_AUTHREQ_MITM |*/ BLE_SM_PAIR_AUTHREQ_SC);

    pServer = NimBLEDevice::createServer();
    pServerCallbacks = new ServerCallbacks();
    pServer->setCallbacks(pServerCallbacks);

    pHid = new NimBLEHIDDevice(pServer);

    inputKeyboard  = pHid->inputReport(KEYBOARD_ID);
    outputKeyboard = pHid->outputReport(KEYBOARD_ID);
    inputMediaKeys = pHid->inputReport(MEDIA_KEYS_ID);

    pHid->manufacturer()->setValue("Arduino");
    pHid->pnp(0x02, vid, pid, version);
    pHid->hidInfo(0x00, 0x02);

    pHid->reportMap(
        (uint8_t*)_hidReportDescriptor, sizeof(_hidReportDescriptor));
    pHid->startServices();

    NimBLEAdvertising* pAdvertising = NimBLEDevice::getAdvertising();
    pAdvertising->setAppearance(HID_KEYBOARD);
    /** Add the services to the advertisment data **/
    pAdvertising->addServiceUUID(pHid->hidService()->getUUID());
    /** If your device is battery powered you may consider setting scan response
     *  to false as it will extend battery life at the expense of less data
     * sent.
     */
    pAdvertising->setScanResponse(false);
    pAdvertising->start();
    pHid->setBatteryLevel(89);

    Serial.println("Advertising Started");
}

void loop()
{
    /** Do your thing here, this just spams notifications to all connected
     * clients */
    // if(pServer->getConnectedCount()) {
    // }
    pServerCallbacks->print_peer_info();
    pServerCallbacks->print_bond_info();

    delay(5000);
}