diff --git a/src/components/ble/NimbleController.cpp b/src/components/ble/NimbleController.cpp index 01901e0a..ec411989 100644 --- a/src/components/ble/NimbleController.cpp +++ b/src/components/ble/NimbleController.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #undef max #undef min #include @@ -16,6 +17,7 @@ #include "components/ble/BleController.h" #include "components/ble/NotificationManager.h" #include "components/datetime/DateTimeController.h" +#include "components/fs/FS.h" #include "systemtask/SystemTask.h" using namespace Pinetime::Controllers; @@ -27,7 +29,8 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask, Controllers::Battery& batteryController, Pinetime::Drivers::SpiNorFlash& spiNorFlash, Controllers::HeartRateController& heartRateController, - Controllers::MotionController& motionController) + Controllers::MotionController& motionController, + Pinetime::Controllers::FS& fs) : systemTask {systemTask}, bleController {bleController}, dateTimeController {dateTimeController}, @@ -43,7 +46,8 @@ NimbleController::NimbleController(Pinetime::System::SystemTask& systemTask, batteryInformationService {batteryController}, immediateAlertService {systemTask, notificationManager}, heartRateService {systemTask, heartRateController}, - motionService{systemTask, motionController}, + fs {fs}, + motionService {systemTask, motionController}, serviceDiscovery({¤tTimeClient, &alertNotificationClient}) { } @@ -123,6 +127,8 @@ void NimbleController::Init() { rc = ble_gatts_start(); ASSERT(rc == 0); + RestoreBond(); + if (!ble_gap_adv_active() && !bleController.IsConnected()) StartAdvertising(); } @@ -202,6 +208,10 @@ int NimbleController::OnGAPEvent(ble_gap_event* event) { /* Connection terminated; resume advertising. */ NRF_LOG_INFO("Disconnect event : BLE_GAP_EVENT_DISCONNECT"); NRF_LOG_INFO("disconnect reason=%d", event->disconnect.reason); + + if (event->disconnect.conn.sec_state.bonded) + PersistBond(event->disconnect.conn); + currentTimeClient.Reset(); alertNotificationClient.Reset(); connectionHandle = BLE_HS_CONN_HANDLE_NONE; @@ -230,6 +240,19 @@ int NimbleController::OnGAPEvent(ble_gap_event* event) { /* Encryption has been enabled or disabled for this connection. */ NRF_LOG_INFO("Security event : BLE_GAP_EVENT_ENC_CHANGE"); NRF_LOG_INFO("encryption change event; status=%0X ", event->enc_change.status); + + if (event->enc_change.status == 0) { + struct ble_gap_conn_desc desc; + ble_gap_conn_find(event->enc_change.conn_handle, &desc); + if (desc.sec_state.bonded) + PersistBond(desc); + + NRF_LOG_INFO("new state: encrypted=%d authenticated=%d bonded=%d key_size=%d", + desc.sec_state.encrypted, + desc.sec_state.authenticated, + desc.sec_state.bonded, + desc.sec_state.key_size); + } break; case BLE_GAP_EVENT_PASSKEY_ACTION: @@ -258,15 +281,13 @@ int NimbleController::OnGAPEvent(ble_gap_event* event) { event->subscribe.cur_notify, event->subscribe.prev_indicate); - if(event->subscribe.reason == BLE_GAP_SUBSCRIBE_REASON_TERM) { + if (event->subscribe.reason == BLE_GAP_SUBSCRIBE_REASON_TERM) { heartRateService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle); motionService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle); - } - else if(event->subscribe.prev_notify == 0 && event->subscribe.cur_notify == 1) { + } else if (event->subscribe.prev_notify == 0 && event->subscribe.cur_notify == 1) { heartRateService.SubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle); motionService.SubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle); - } - else if(event->subscribe.prev_notify == 1 && event->subscribe.cur_notify == 0) { + } else if (event->subscribe.prev_notify == 1 && event->subscribe.cur_notify == 0) { heartRateService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle); motionService.UnsubscribeNotification(event->subscribe.conn_handle, event->subscribe.attr_handle); } @@ -340,3 +361,81 @@ void NimbleController::NotifyBatteryLevel(uint8_t level) { batteryInformationService.NotifyBatteryLevel(connectionHandle, level); } } + +void NimbleController::PersistBond(struct ble_gap_conn_desc& desc) { + union ble_store_key key; + union ble_store_value our_sec, peer_sec, peer_cccd_set[MYNEWT_VAL(BLE_STORE_MAX_CCCDS)] = {0}; + int rc; + + memset(&key, 0, sizeof key); + memset(&our_sec, 0, sizeof our_sec); + key.sec.peer_addr = desc.peer_id_addr; + rc = ble_store_read_our_sec(&key.sec, &our_sec.sec); + + if (memcmp(&our_sec.sec, &bondId, sizeof bondId) == 0) + return; + + memcpy(&bondId, &our_sec.sec, sizeof bondId); + + memset(&key, 0, sizeof key); + memset(&peer_sec, 0, sizeof peer_sec); + key.sec.peer_addr = desc.peer_id_addr; + rc += ble_store_read_peer_sec(&key.sec, &peer_sec.sec); + + if (rc == 0) { + memset(&key, 0, sizeof key); + key.cccd.peer_addr = desc.peer_id_addr; + int peer_count = 0; + ble_store_util_count(BLE_STORE_OBJ_TYPE_CCCD, &peer_count); + for (int i = 0; i < peer_count; i++) { + key.cccd.idx = peer_count; + ble_store_read_cccd(&key.cccd, &peer_cccd_set[i].cccd); + } + + /* Wakeup Spi and SpiNorFlash before accessing the file system + * This should be fixed in the FS driver + */ + systemTask.PushMessage(Pinetime::System::Messages::GoToRunning); + systemTask.PushMessage(Pinetime::System::Messages::DisableSleeping); + vTaskDelay(10); + + lfs_file_t file_p; + + rc = fs.FileOpen(&file_p, "/bond.dat", LFS_O_WRONLY | LFS_O_CREAT); + if (rc == 0) { + fs.FileWrite(&file_p, reinterpret_cast(&our_sec.sec), sizeof our_sec); + fs.FileWrite(&file_p, reinterpret_cast(&peer_sec.sec), sizeof peer_sec); + fs.FileWrite(&file_p, reinterpret_cast(&peer_count), 1); + for (int i = 0; i < peer_count; i++) { + fs.FileWrite(&file_p, reinterpret_cast(&peer_cccd_set[i].cccd), sizeof(struct ble_store_value_cccd)); + } + fs.FileClose(&file_p); + } + systemTask.PushMessage(Pinetime::System::Messages::EnableSleeping); + } +} + +void NimbleController::RestoreBond() { + lfs_file_t file_p; + union ble_store_value sec, cccd; + uint8_t peer_count = 0; + + if (fs.FileOpen(&file_p, "/bond.dat", LFS_O_RDONLY) == 0) { + memset(&sec, 0, sizeof sec); + fs.FileRead(&file_p, reinterpret_cast(&sec.sec), sizeof sec); + ble_store_write_our_sec(&sec.sec); + + memset(&sec, 0, sizeof sec); + fs.FileRead(&file_p, reinterpret_cast(&sec.sec), sizeof sec); + ble_store_write_peer_sec(&sec.sec); + + fs.FileRead(&file_p, &peer_count, 1); + for (int i = 0; i < peer_count; i++) { + fs.FileRead(&file_p, reinterpret_cast(&cccd.cccd), sizeof(struct ble_store_value_cccd)); + ble_store_write_cccd(&cccd.cccd); + } + + fs.FileClose(&file_p); + fs.FileDelete("/bond.dat"); + } +} diff --git a/src/components/ble/NimbleController.h b/src/components/ble/NimbleController.h index 895b87f2..944e8cad 100644 --- a/src/components/ble/NimbleController.h +++ b/src/components/ble/NimbleController.h @@ -20,6 +20,7 @@ #include "components/ble/ServiceDiscovery.h" #include "components/ble/HeartRateService.h" #include "components/ble/MotionService.h" +#include "components/fs/FS.h" namespace Pinetime { namespace Drivers { @@ -45,7 +46,8 @@ namespace Pinetime { Controllers::Battery& batteryController, Pinetime::Drivers::SpiNorFlash& spiNorFlash, Controllers::HeartRateController& heartRateController, - Controllers::MotionController& motionController); + Controllers::MotionController& motionController, + Pinetime::Controllers::FS& fs); void Init(); void StartAdvertising(); int OnGAPEvent(ble_gap_event* event); @@ -78,6 +80,9 @@ namespace Pinetime { fastAdvCount = 0; } + void PersistBond(struct ble_gap_conn_desc &desc); + void RestoreBond(); + private: static constexpr const char* deviceName = "InfiniTime"; Pinetime::System::SystemTask& systemTask; @@ -98,10 +103,12 @@ namespace Pinetime { ImmediateAlertService immediateAlertService; HeartRateService heartRateService; MotionService motionService; + Pinetime::Controllers::FS& fs; uint8_t addrType; // 1 = Random, 0 = PUBLIC uint16_t connectionHandle = BLE_HS_CONN_HANDLE_NONE; uint8_t fastAdvCount = 0; + uint8_t bondId[16] = {0}; ble_uuid128_t dfuServiceUuid { .u {.type = BLE_UUID_TYPE_128}, @@ -110,6 +117,6 @@ namespace Pinetime { ServiceDiscovery serviceDiscovery; }; - static NimbleController* nptr; + static NimbleController* nptr; } } diff --git a/src/systemtask/SystemTask.cpp b/src/systemtask/SystemTask.cpp index 2fb4de51..215c78a5 100644 --- a/src/systemtask/SystemTask.cpp +++ b/src/systemtask/SystemTask.cpp @@ -109,7 +109,8 @@ SystemTask::SystemTask(Drivers::SpiMaster& spi, batteryController, spiNorFlash, heartRateController, - motionController) { + motionController, + fs) { } void SystemTask::Start() {