Fix Firmata crash, J-791. Peripheral discover profile failure, J-788

libraries/CurieBLE/examples/peripheral/broadcast/broadcast.ino

@@ -0,0 +1,31 @@
+#include <CurieBLE.h>
+
+uint8_t value = 0;
+
+BLEPeripheral peripheral;
+BLEService service = BLEService("EEE0");
+BLEShortCharacteristic characteristic = BLEShortCharacteristic("EEE1", BLERead | BLENotify | BLEBroadcast);
+
+void setup() {
+  Serial.begin(9600);
+
+  peripheral.setLocalName("BLEBroadcast");
+  peripheral.setAdvertisedServiceUuid(service.uuid());
+
+  peripheral.addAttribute(service);
+  peripheral.addAttribute(characteristic);
+
+  characteristic.setValue(value);
+
+  peripheral.begin();
+  characteristic.broadcast();
+
+  Serial.println(F("BLE Broadcast Count"));
+}
+
+void loop() {
+    peripheral.poll();
+    characteristic.setValue(value);    
+    delay(1000);
+    value++;
+}

libraries/CurieBLE/src/BLEPeripheral.cpp

@@ -21,7 +21,7 @@
 
 #include "BLEPeripheral.h"
 
-static BLEPeripheralEventHandler m_eventHandlers[BLEDeviceLastEvent];
+static BLEPeripheralEventHandler m_eventHandlers[BLEDeviceLastEvent] = {NULL, NULL, NULL, NULL};
 
 void bleBackCompatiblePeripheralConnectHandler(BLEDevice central) 
 {
@@ -146,15 +146,15 @@ BLECentral BLEPeripheral::central(void)
 
 bool BLEPeripheral::connected(void)
 {
-    return BLE.connected();
+    BLEDevice centralBle = BLE.central();
+    return centralBle.connected();
 }
 
 void BLEPeripheral::init()
 {
     if (!_initCalled)
     {
         BLE.begin();
-        memset(m_eventHandlers, 0, sizeof(m_eventHandlers));
         _initCalled = true;
     }
 }

libraries/CurieBLE/src/internal/BLECallbacks.cpp

@@ -172,6 +172,23 @@ uint8_t profile_service_read_rsp_process(bt_conn_t *conn,
     return ret;
 }
 
+uint8_t profile_characteristic_read_rsp_process(bt_conn_t *conn, 
+                                                 int err,
+                                                 bt_gatt_read_params_t *params,
+                                                 const void *data, 
+                                                 uint16_t length)
+{
+    BLEDevice bleDevice(bt_conn_get_dst(conn));
+    BLEServiceImp* service_imp = BLEProfileManager::instance()->getServiceBySubHandle(bleDevice, params->single.handle);
+
+    uint8_t ret = service_imp->characteristicReadRspProc(conn, 
+                                                         err,
+                                                         params,
+                                                         data,
+                                                         length);
+    pr_debug(LOG_MODULE_BLE, "%s-%d:ret-%d", __FUNCTION__, __LINE__, ret);
+    return ret;
+}
 
 
 void bleConnectEventHandler(bt_conn_t *conn, 

libraries/CurieBLE/src/internal/BLECallbacks.h

@@ -76,6 +76,11 @@ uint8_t profile_service_read_rsp_process(bt_conn_t *conn,
 
 void ble_on_write_no_rsp_complete(struct bt_conn *conn, uint8_t err,
                                          const void *data);
+uint8_t profile_characteristic_read_rsp_process(bt_conn_t *conn, 
+                                                 int err,
+                                                 bt_gatt_read_params_t *params,
+                                                 const void *data, 
+                                                 uint16_t length);
 
 #endif
 

libraries/CurieBLE/src/internal/BLEProfileManager.cpp

@@ -507,6 +507,40 @@ BLEServiceImp* BLEProfileManager::service(const BLEDevice &bledevice, int index)
     return serviceImp;
 }
 
+BLEServiceImp* BLEProfileManager::getServiceBySubHandle(const BLEDevice &bledevice, uint16_t handle) const
+{
+    BLEServiceImp* serviceImp = NULL;
+    uint16_t start_handle;
+    uint16_t end_handle;
+    #if 1
+    const BLEServiceLinkNodeHeader* serviceHeader = getServiceHeader(bledevice);
+    if (NULL == serviceHeader)
+    {
+        // Doesn't find the service
+        return NULL;
+    }
+    BLEServiceNodePtr node = serviceHeader->next;
+
+    while (node != NULL)
+    {
+        serviceImp = node->value;
+        start_handle = serviceImp->startHandle();
+        end_handle = serviceImp->endHandle();
+        if (handle >= start_handle && handle <= end_handle)
+        {
+            break;
+        }
+        node = node->next;
+    }
+
+    if (NULL == node)
+    {
+        serviceImp = NULL;
+    }
+    #endif
+    return serviceImp;
+}
+
 void BLEProfileManager::handleConnectedEvent(const bt_addr_le_t* deviceAddr)
 {
     int index = getUnusedIndex();

libraries/CurieBLE/src/internal/BLEProfileManager.h

@@ -98,6 +98,7 @@ class BLEProfileManager{
     BLEServiceImp* service(const BLEDevice &bledevice, const char * uuid) const;
     BLEServiceImp* service(const BLEDevice &bledevice, int index) const;
     BLEServiceImp* service(const BLEDevice &bledevice, const bt_uuid_t* uuid) const;
+    BLEServiceImp* getServiceBySubHandle(const BLEDevice &bledevice, uint16_t handle) const;
     int serviceCount(const BLEDevice &bledevice) const;
     int characteristicCount(const BLEDevice &bledevice) const;