Merge pull request #13622 from pan-/cordio-configure-identity-address

pan- · web-flow · commit d6c5b2fcd677 · 2020-09-21T10:25:52.000+01:00
Cordio configure identity address
diff --git a/connectivity/FEATURE_BLE/include/ble/Gap.h b/connectivity/FEATURE_BLE/include/ble/Gap.h
@@ -1448,6 +1448,8 @@ class Gap {
      * forbidden by the Bluetooth specification.
      */
     ble_error_t setRandomStaticAddress(const ble::address_t& address);
+
+    ble::address_t getRandomStaticAddress();
 #endif // !defined(DOXYGEN_ONLY)
 
 private:
diff --git a/connectivity/FEATURE_BLE/libraries/cordio_stack/ble-host/include/dm_api.h b/connectivity/FEATURE_BLE/libraries/cordio_stack/ble-host/include/dm_api.h
@@ -3035,6 +3035,18 @@ void DmSecSetLocalCsrk(uint8_t *pCsrk);
 /*************************************************************************************************/
 void DmSecSetLocalIrk(uint8_t *pIrk);
 
+/*************************************************************************************************/
+/*!
+ *  \brief  This function sets the local identity address used by the device.
+ *
+ *  \param  pAddr     Pointer to the address.
+ *  \param  type      Type of the address.
+ *
+ *  \return None.
+ */
+/*************************************************************************************************/
+void DmSecSetLocalIdentityAddr(const uint8_t *pAddr, uint8_t type);
+
 /*************************************************************************************************/
 /*!
  *  \brief  This function generates an ECC key for use with LESC security.
@@ -3345,6 +3357,25 @@ uint8_t *DmSecGetLocalCsrk(void);
 /*************************************************************************************************/
 uint8_t *DmSecGetLocalIrk(void);
 
+/*************************************************************************************************/
+/*!
+ *  \brief  For internal use only.  This function gets the local identity address used by the device.
+ *
+ *  \return Pointer to the identity address.
+ */
+/*************************************************************************************************/
+uint8_t *DmSecGetLocalIdentityAddr(void);
+
+/*************************************************************************************************/
+/*!
+ *  \brief  For internal use only.  This function gets the local identity address type used by the
+ *  device.
+ *
+ *  \return The identity address type.
+ */
+/*************************************************************************************************/
+uint8_t DmSecGetLocalIdentityAddrType(void);
+
 /*************************************************************************************************/
 /*!
  *  \brief  For internal use only.  Read the features of the remote device.
diff --git a/connectivity/FEATURE_BLE/libraries/cordio_stack/ble-host/sources/stack/dm/dm_sec.c b/connectivity/FEATURE_BLE/libraries/cordio_stack/ble-host/sources/stack/dm/dm_sec.c
@@ -329,6 +329,7 @@ void DmSecInit(void)
   dmFcnIfTbl[DM_ID_SEC] = (dmFcnIf_t *) &dmSecFcnIf;
 
   dmSecCb.pCsrk = dmSecCb.pIrk = (uint8_t *) calc128Zeros;
+  dmSecCb.addrType = DM_ADDR_NONE;
 }
 
 /*************************************************************************************************/
@@ -363,6 +364,24 @@ void DmSecSetLocalIrk(uint8_t *pIrk)
   WsfTaskUnlock();
 }
 
+/*************************************************************************************************/
+/*!
+ *  \brief  This function sets the local identity address used by the device.
+ *
+ *  \param  pAddr     Pointer to the address.
+ *  \param  type      Type of the address.
+ *
+ *  \return None.
+ */
+/*************************************************************************************************/
+void DmSecSetLocalIdentityAddr(const uint8_t *pAddr, uint8_t type)
+{
+  WsfTaskLock();
+  dmSecCb.addrType = type;
+  BdaCpy(dmSecCb.bdAddr, pAddr);
+  WsfTaskUnlock();
+}
+
 /*************************************************************************************************/
 /*!
  *  \brief  This function gets the local CSRK used by the device.
@@ -387,6 +406,39 @@ uint8_t *DmSecGetLocalIrk(void)
   return dmSecCb.pIrk;
 }
 
+/*************************************************************************************************/
+/*!
+ *  \brief  For internal use only.  This function gets the local identity address used by the device.
+ *
+ *  \return Pointer to the identity address.
+ */
+/*************************************************************************************************/
+uint8_t *DmSecGetLocalIdentityAddr(void)
+{
+  if (dmSecCb.addrType != DM_ADDR_NONE) {
+    return dmSecCb.bdAddr;
+  } else {
+    return HciGetBdAddr();
+  }
+}
+
+/*************************************************************************************************/
+/*!
+ *  \brief  For internal use only.  This function gets the local identity address type used by the
+ *  device.
+ *
+ *  \return The identity address type.
+ */
+/*************************************************************************************************/
+uint8_t DmSecGetLocalIdentityAddrType(void)
+{
+  if (dmSecCb.addrType != DM_ADDR_NONE) {
+    return dmSecCb.addrType;
+  } else {
+    return DM_ADDR_PUBLIC;
+  }
+}
+
 /*************************************************************************************************/
 /*!
  *  \brief  Reset the sec module.
diff --git a/connectivity/FEATURE_BLE/libraries/cordio_stack/ble-host/sources/stack/dm/dm_sec.h b/connectivity/FEATURE_BLE/libraries/cordio_stack/ble-host/sources/stack/dm/dm_sec.h
@@ -83,6 +83,8 @@ typedef struct
 {
   uint8_t               *pIrk;
   uint8_t               *pCsrk;
+  bdAddr_t              bdAddr;
+  uint8_t               addrType;
 } dmSecCb_t;
 
 /**************************************************************************************************
diff --git a/connectivity/FEATURE_BLE/libraries/cordio_stack/ble-host/sources/stack/smp/smp_act.c b/connectivity/FEATURE_BLE/libraries/cordio_stack/ble-host/sources/stack/smp/smp_act.c
@@ -567,8 +567,8 @@ bool_t smpSendKey(smpCcb_t *pCcb, uint8_t keyDist)
     {
       /* send second part of IRK */
       UINT8_TO_BSTREAM(p, SMP_CMD_ID_ADDR_INFO);
-      UINT8_TO_BSTREAM(p, DM_ADDR_PUBLIC);
-      BDA_TO_BSTREAM(p, HciGetBdAddr());
+      UINT8_TO_BSTREAM(p, DmSecGetLocalIdentityAddrType());
+      BDA_TO_BSTREAM(p, DmSecGetLocalIdentityAddr());
 
     }
     else if ((keyDist & SMP_KEY_DIST_SIGN) &&
diff --git a/connectivity/FEATURE_BLE/source/Gap.cpp b/connectivity/FEATURE_BLE/source/Gap.cpp
@@ -501,4 +501,9 @@ ble_error_t Gap::setRandomStaticAddress(const ble::address_t &address)
     return impl->setRandomStaticAddress(address);
 }
 
+ble::address_t Gap::getRandomStaticAddress()
+{
+    return impl->getRandomStaticAddress();
+}
+
 } // namespace ble
diff --git a/connectivity/FEATURE_BLE/source/cordio/source/PalSecurityManagerImpl.cpp b/connectivity/FEATURE_BLE/source/cordio/source/PalSecurityManagerImpl.cpp
@@ -220,21 +220,6 @@ ble_error_t PalSecurityManager::set_private_address_timeout(
     return BLE_ERROR_NONE;
 }
 
-/**
- * @see ::ble::PalSecurityManager::get_identity_address
- */
-
-ble_error_t PalSecurityManager::get_identity_address(
-    address_t &address,
-    bool &public_address
-)
-{
-    // On cordio, the public address is hardcoded as the identity address.
-    address = address_t(HciGetBdAddr());
-    public_address = true;
-    return BLE_ERROR_NONE;
-}
-
 ////////////////////////////////////////////////////////////////////////////
 // Keys
 //
@@ -288,6 +273,17 @@ ble_error_t PalSecurityManager::set_irk(const irk_t &irk)
     return BLE_ERROR_NONE;
 }
 
+ble_error_t PalSecurityManager::set_identity_address(
+    const address_t &address,
+    bool public_address
+)
+{
+    DmSecSetLocalIdentityAddr(
+        address.data(),
+        public_address ? DM_ADDR_PUBLIC : DM_ADDR_RANDOM
+    );
+    return BLE_ERROR_NONE;
+}
 
 ble_error_t PalSecurityManager::set_csrk(
     const csrk_t &csrk,
diff --git a/connectivity/FEATURE_BLE/source/cordio/source/PalSecurityManagerImpl.h b/connectivity/FEATURE_BLE/source/cordio/source/PalSecurityManagerImpl.h
@@ -183,11 +183,6 @@ class PalSecurityManager final : public ble::PalSecurityManager {
      */
     ble_error_t set_private_address_timeout(uint16_t timeout_in_seconds) final;
 
-    /**
-     * @see ::ble::PalSecurityManager::get_identity_address
-     */
-    ble_error_t get_identity_address(address_t &address, bool &public_address) final;
-
     ////////////////////////////////////////////////////////////////////////////
     // Keys
     //
@@ -214,6 +209,13 @@ class PalSecurityManager final : public ble::PalSecurityManager {
      */
     ble_error_t set_irk(const irk_t &irk) final;
 
+    /**
+     * @see ::ble::PalSecurityManager::set_identity_address
+     */
+    ble_error_t set_identity_address(
+        const address_t &address, bool public_address
+    ) final;
+
     /**
      * @see ::ble::PalSecurityManager::set_csrk
      */
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -415,6 +415,10 @@ ble_error_t Gap::setRandomStaticAddress(
     return BLE_ERROR_NONE;
 }
 
+ble::address_t Gap::getRandomStaticAddress()
+{
+    return _random_static_identity_address;
+}
 
 ble_error_t Gap::getAddress(
     own_address_type_t &type,
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.h b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
@@ -346,6 +346,8 @@ class Gap :
      */
     ble_error_t setRandomStaticAddress(const ble::address_t &address);
 
+    ble::address_t getRandomStaticAddress();
+
 #endif // !defined(DOXYGEN_ONLY)
 
     /* ===================================================================== */
diff --git a/connectivity/FEATURE_BLE/source/generic/SecurityDb.h b/connectivity/FEATURE_BLE/source/generic/SecurityDb.h
@@ -450,6 +450,20 @@ class SecurityDb {
         return _local_identity.irk;
     }
 
+    /**
+     * Return local identity address.
+     */
+    virtual const address_t& get_local_identity_address() {
+        return _local_identity.identity_address;
+    }
+
+    /**
+     * Return if the local identity address is public or not
+     */
+    virtual bool is_local_identity_address_public() {
+        return _local_identity.identity_address_is_public;
+    }
+
     /* list management */
 
     /**
diff --git a/connectivity/FEATURE_BLE/source/generic/SecurityManagerImpl.cpp b/connectivity/FEATURE_BLE/source/generic/SecurityManagerImpl.cpp
@@ -1032,23 +1032,36 @@ ble_error_t SecurityManager::init_identity()
     if (!_db) return BLE_ERROR_INITIALIZATION_INCOMPLETE;
     const irk_t *pirk = nullptr;
 
+    ble::Gap& gap = BLE::Instance().gap();
+
     irk_t irk = _db->get_local_irk();
     if (irk != irk_t()) {
         pirk = &irk;
+
+        if (!_db->is_local_identity_address_public()) {
+            // Some controllers doesn't store their random static address and
+            // instead generates them at each reboot.
+            // The code should replace the random static address with the identity
+            // address if this is the case.
+            if (_db->get_local_identity_address() != gap.getRandomStaticAddress()) {
+                ble_error_t err = gap.setRandomStaticAddress(_db->get_local_identity_address());
+                if (err) {
+                    return err;
+                }
+            }
+        }
     } else {
         ble_error_t ret = get_random_data(irk.data(), irk.size());
         if (ret != BLE_ERROR_NONE) {
             return ret;
         }
 
         pirk = &irk;
-        address_t identity_address;
-        bool public_address;
-        ret = _pal.get_identity_address(identity_address, public_address);
+        address_t random_static_address = gap.getRandomStaticAddress();
         if (ret != BLE_ERROR_NONE) {
             return ret;
         }
-        _db->set_local_identity(irk, identity_address, public_address);
+        _db->set_local_identity(irk, random_static_address, /* public_address */ false);
     }
 
     auto err = _pal.set_irk(*pirk);
diff --git a/connectivity/FEATURE_BLE/source/pal/PalSecurityManager.h b/connectivity/FEATURE_BLE/source/pal/PalSecurityManager.h
@@ -761,20 +761,6 @@ class PalSecurityManager {
         uint16_t timeout_in_seconds
     ) = 0;
 
-    /**
-     * Retrieve the identity address used by the controller
-     *
-     * @param address Will contain the address retrieved.
-     * @param public_address will be true if the address is public and false
-     * otherwise.
-     * @return BLE_ERROR_NONE On success, else an error code indicating the reason
-     * of the failure
-     */
-    virtual ble_error_t get_identity_address(
-        address_t& address,
-        bool& public_address
-    ) = 0;
-
     ////////////////////////////////////////////////////////////////////////////
     // Keys
     //
@@ -814,6 +800,18 @@ class PalSecurityManager {
     virtual ble_error_t set_irk(
         const irk_t &irk
     ) = 0;
+
+    /**
+     * Set the local identity address.
+     *
+     * @param[in] address identity address of the device
+     * @param[in] public_address true if a public address is used and false otherwise.
+     * @return BLE_ERROR_NONE On success, else an error code indicating reason for failure
+     */
+    virtual ble_error_t set_identity_address(
+        const address_t &address, bool public_address
+    ) = 0;
+
     /**
      * Set the local CSRK.
      *

Original file line number	Diff line number	Diff line change
`@@ -83,6 +83,8 @@ typedef struct`
`83`	`83`	`{`
`84`	`84`	`uint8_t *pIrk;`
`85`	`85`	`uint8_t *pCsrk;`
	`86`	`+ bdAddr_t bdAddr;`
	`87`	`+ uint8_t addrType;`
`86`	`88`	`} dmSecCb_t;`
`87`	`89`
`88`	`90`	`/**************************************************************************************************`
Original file line number	Diff line number	Diff line change
`@@ -567,8 +567,8 @@ bool_t smpSendKey(smpCcb_t *pCcb, uint8_t keyDist)`
`567`	`567`	`{`
`568`	`568`	`/* send second part of IRK */`
`569`	`569`	`UINT8_TO_BSTREAM(p, SMP_CMD_ID_ADDR_INFO);`
`570`		`- UINT8_TO_BSTREAM(p, DM_ADDR_PUBLIC);`
`571`		`- BDA_TO_BSTREAM(p, HciGetBdAddr());`
	`570`	`+ UINT8_TO_BSTREAM(p, DmSecGetLocalIdentityAddrType());`
	`571`	`+ BDA_TO_BSTREAM(p, DmSecGetLocalIdentityAddr());`
`572`	`572`
`573`	`573`	`}`
`574`	`574`	`else if ((keyDist & SMP_KEY_DIST_SIGN) &&`
Original file line number	Diff line number	Diff line change
`@@ -501,4 +501,9 @@ ble_error_t Gap::setRandomStaticAddress(const ble::address_t &address)`
`501`	`501`	`return impl->setRandomStaticAddress(address);`
`502`	`502`	`}`
`503`	`503`
	`504`	`+ble::address_t Gap::getRandomStaticAddress()`
	`505`	`+{`
	`506`	`+ return impl->getRandomStaticAddress();`
	`507`	`+}`
	`508`	`+`
`504`	`509`	`} // namespace ble`
Original file line number	Diff line number	Diff line change
`@@ -415,6 +415,10 @@ ble_error_t Gap::setRandomStaticAddress(`
`415`	`415`	`return BLE_ERROR_NONE;`
`416`	`416`	`}`
`417`	`417`
	`418`	`+ble::address_t Gap::getRandomStaticAddress()`
	`419`	`+{`
	`420`	`+ return _random_static_identity_address;`
	`421`	`+}`
`418`	`422`
`419`	`423`	`ble_error_t Gap::getAddress(`
`420`	`424`	`own_address_type_t &type,`