From 980df5ae597b62422bc96e5db06c1e67bca55cab Mon Sep 17 00:00:00 2001
From: Giampaolo Mancini <giampaolo@trampolineup.com>
Date: Sat, 13 Jun 2020 11:58:35 +0200
Subject: [PATCH] Fix slow handling of RX data

---
 .../ScanCallbackInterleaved.ino               | 80 ++++++++++++++++++
 .../ScanInterleaved/ScanInterleaved.ino       | 84 +++++++++++++++++++
 src/utility/HCICordioTransport.cpp            |  6 +-
 3 files changed, 167 insertions(+), 3 deletions(-)
 create mode 100644 examples/Central/ScanCallbackInterleaved/ScanCallbackInterleaved.ino
 create mode 100644 examples/Central/ScanInterleaved/ScanInterleaved.ino

diff --git a/examples/Central/ScanCallbackInterleaved/ScanCallbackInterleaved.ino b/examples/Central/ScanCallbackInterleaved/ScanCallbackInterleaved.ino
new file mode 100644
index 00000000..ab95612c
--- /dev/null
+++ b/examples/Central/ScanCallbackInterleaved/ScanCallbackInterleaved.ino
@@ -0,0 +1,80 @@
+/*
+  Scan Callback
+
+  This example scans for BLE peripherals and prints out their advertising details:
+  address, local name, adverised service UUIDs. Unlike the Scan example, it uses
+  the callback style APIs and disables filtering so the peripheral discovery is
+  reported for every single advertisement it makes.
+
+  The circuit:
+  - Arduino MKR WiFi 1010, Arduino Uno WiFi Rev2 board, Arduino Nano 33 IoT,
+    Arduino Nano 33 BLE, or Arduino Nano 33 BLE Sense board.
+
+  This example code is in the public domain.
+*/
+
+#include <ArduinoBLE.h>
+
+unsigned long eventNow;
+unsigned long eventInt = { 2000 } ;
+const unsigned int eventStep = { 250 };
+unsigned int eventDelay = { 1000 };
+const unsigned int delayStep = { 100 };
+
+unsigned int peripheralsCount = { 0 };
+
+void setup() {
+  Serial.begin(9600);
+  while (!Serial);
+
+  // begin initialization
+  if (!BLE.begin()) {
+    Serial.println("starting BLE failed!");
+
+    while (1);
+  }
+
+  Serial.println("BLE Central scan callback");
+
+  // set the discovered event handle
+  BLE.setEventHandler(BLEDiscovered, bleCentralDiscoverHandler);
+
+  // start scanning for peripherals with duplicates
+  BLE.scan(true);
+}
+
+void loop() {
+  // poll the central for events
+  BLE.poll();
+
+  if (millis() > eventNow) {
+    Serial.print("Event 1 - ");
+    Serial.print("Peripherals: ");
+    Serial.print(peripheralsCount);
+    peripheralsCount = 0;
+    
+    Serial.print(" Delay: ");
+    Serial.print(eventDelay);
+    
+    Serial.print(" Interval: ");
+    Serial.print(eventInt);
+
+    delay(eventDelay);
+    eventDelay += delayStep;
+    eventNow = millis() + eventInt;
+    eventInt += eventStep;
+    
+    Serial.print(" - Done.");
+    Serial.println();
+  }  
+}
+
+void bleCentralDiscoverHandler(BLEDevice peripheral) {
+  BLE.stopScan();
+  peripheralsCount++;
+
+  digitalWrite(LED_BUILTIN, HIGH);
+  delay(5);
+  digitalWrite(LED_BUILTIN, LOW);
+  BLE.scan(true);
+}
diff --git a/examples/Central/ScanInterleaved/ScanInterleaved.ino b/examples/Central/ScanInterleaved/ScanInterleaved.ino
new file mode 100644
index 00000000..9fca46c0
--- /dev/null
+++ b/examples/Central/ScanInterleaved/ScanInterleaved.ino
@@ -0,0 +1,84 @@
+/*
+  Scan
+
+  This example scans for BLE peripherals and prints out their advertising details:
+  address, local name, adverised service UUID's.
+
+  The circuit:
+  - Arduino MKR WiFi 1010, Arduino Uno WiFi Rev2 board, Arduino Nano 33 IoT,
+    Arduino Nano 33 BLE, or Arduino Nano 33 BLE Sense board.
+
+  This example code is in the public domain.
+*/
+
+#include <ArduinoBLE.h>
+
+unsigned long eventNow;
+unsigned long eventInt = { 2000 } ;
+const unsigned int eventStep = { 250 };
+unsigned int eventDelay = { 1000 };
+const unsigned int delayStep = { 100 };
+
+unsigned int peripheralsCount = { 0 };
+
+
+void setup() {
+  Serial.begin(9600);
+  while (!Serial);
+
+  delay(2000);
+
+  // BLE.debug(Serial);
+
+  // begin initialization
+  if (!BLE.begin()) {
+    Serial.println("starting BLE failed!");
+
+    while (1);
+  }
+
+  Serial.println("BLE Central scan");
+
+
+  // start scanning for peripheral
+  BLE.scan();
+
+  eventNow = millis() + eventInt;
+}
+
+void loop() {
+  // check if a peripheral has been discovered
+  BLEDevice peripheral = BLE.available();
+
+  if (peripheral) {
+    BLE.stopScan();
+    peripheralsCount++;
+
+    digitalWrite(LED_BUILTIN, HIGH);
+    delay(5);
+    digitalWrite(LED_BUILTIN, LOW);
+
+    BLE.scan();
+  }
+
+  if (millis() > eventNow) {
+    Serial.print("Event 1 - ");
+    Serial.print("Peripherals: ");
+    Serial.print(peripheralsCount);
+    peripheralsCount = 0;
+    
+    Serial.print(" Delay: ");
+    Serial.print(eventDelay);
+    
+    Serial.print(" Interval: ");
+    Serial.print(eventInt);
+
+    delay(eventDelay);
+    eventDelay += delayStep;
+    eventNow = millis() + eventInt;
+    eventInt += eventStep;
+    
+    Serial.print(" - Done.");
+    Serial.println();
+  }
+}
diff --git a/src/utility/HCICordioTransport.cpp b/src/utility/HCICordioTransport.cpp
index 15f0e128..59818e25 100644
--- a/src/utility/HCICordioTransport.cpp
+++ b/src/utility/HCICordioTransport.cpp
@@ -258,9 +258,9 @@ size_t HCICordioTransportClass::write(const uint8_t* data, size_t length)
 
 void HCICordioTransportClass::handleRxData(uint8_t* data, uint8_t len)
 {
-  if (_rxBuf.availableForStore() < len) {
-    // drop!
-    return;
+  while (_rxBuf.availableForStore() < len) {
+    // Wait for free space on RingBuffer
+    yield();
   }
 
   for (int i = 0; i < len; i++) {