Fix sporadic hangup when stopping notifies

Author: Matthias Spiller

libraries/Bluefruit52Lib/src/BLECharacteristic.cpp

@@ -682,7 +682,12 @@ bool BLECharacteristic::notify(uint16_t conn_hdl, const void* data, uint16_t len
       };
 
       LOG_LV2("CHR", "Notify %d bytes", packet_len);
-      VERIFY_STATUS( sd_ble_gatts_hvx(conn_hdl, &hvx_params), false );
+      uint32_t status = sd_ble_gatts_hvx(conn_hdl, &hvx_params);
+      if(NRF_SUCCESS != status)
+      {
+        conn->releaseHvnPacket();
+      }
+      VERIFY_STATUS(status, false );
 
       remaining -= packet_len;
       u8data    += packet_len;

libraries/Bluefruit52Lib/src/BLEConnection.cpp

@@ -210,6 +210,11 @@ bool BLEConnection::getHvnPacket (void)
   return xSemaphoreTake(_hvn_sem, ms2tick(BLE_GENERIC_TIMEOUT));
 }
 
+bool BLEConnection::releaseHvnPacket(void)
+{
+  return xSemaphoreGive(_hvn_sem);
+}
+
 bool BLEConnection::getWriteCmdPacket (void)
 {
   return xSemaphoreTake(_wrcmd_sem, ms2tick(BLE_GENERIC_TIMEOUT));

libraries/Bluefruit52Lib/src/BLEConnection.h

@@ -100,6 +100,7 @@ class BLEConnection
     void stopRssi(void);
 
     bool getHvnPacket(void);
+    bool releaseHvnPacket(void);
     bool getWriteCmdPacket(void);
     bool waitForIndicateConfirm(void);