Allow BluetoothSerial::connect() with specified channel and more options (#6380)

* BTAddress const, add bool()

* BTAdvertisedDevice: const functions

* BluetoothSerial: add: getChannels, add isClosed, add read/peek timeout, add connect with channel#

* BluetoothSerial: add sec_mask, role in ::connect

* BluetoothSerial add discover and connect with channel number example

* DiscoverConnect: add SPP_ENABLED check

* DiscoverConnect: disable on esp32s3

libraries/BluetoothSerial/examples/DiscoverConnect/DiscoverConnect.ino

+/**
+ * Bluetooth Classic Example
+ * Scan for devices - asyncronously, print device as soon as found
+ * query devices for SPP - SDP profile
+ * connect to first device offering a SPP connection
+ * 
+ * Example python server:
+ * source: https://gist.github.com/ukBaz/217875c83c2535d22a16ba38fc8f2a91
+ *
+ * Tested with Raspberry Pi onboard Wifi/BT, USB BT 4.0 dongles, USB BT 1.1 dongles, 
+ * 202202: does NOT work with USB BT 2.0 dongles when esp32 aduino lib is compiled with SSP support!
+ *         see https://github.com/espressif/esp-idf/issues/8394
+ *         
+ * use ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE in connect() if remote side requests 'RequireAuthentication': dbus.Boolean(True),
+ * use ESP_SPP_SEC_NONE or ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE in connect() if remote side has Authentication: False
+ */
+
+#include <map>
+#include <BluetoothSerial.h>
+
+#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
+#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
+#endif
+
+#if !defined(CONFIG_BT_SPP_ENABLED)
+#error Serial Bluetooth not available or not enabled. It is only available for the ESP32 chip.
+#endif
+
+BluetoothSerial SerialBT;
+
+
+#define BT_DISCOVER_TIME  10000
+esp_spp_sec_t sec_mask=ESP_SPP_SEC_NONE; // or ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE to request pincode confirmation
+esp_spp_role_t role=ESP_SPP_ROLE_SLAVE; // or ESP_SPP_ROLE_MASTER
+
+// std::map<BTAddress, BTAdvertisedDeviceSet> btDeviceList;
+
+void setup() {
+  Serial.begin(115200);
+  if(! SerialBT.begin("ESP32test", true) ) {
+    Serial.println("========== serialBT failed!");
+    abort();
+  }
+  // SerialBT.setPin("1234"); // doesn't seem to change anything
+  // SerialBT.enableSSP(); // doesn't seem to change anything
+
+
+  Serial.println("Starting discoverAsync...");
+  BTScanResults* btDeviceList = SerialBT.getScanResults();  // maybe accessing from different threads!
+  if (SerialBT.discoverAsync([](BTAdvertisedDevice* pDevice) {
+      // BTAdvertisedDeviceSet*set = reinterpret_cast<BTAdvertisedDeviceSet*>(pDevice);
+      // btDeviceList[pDevice->getAddress()] = * set;
+      Serial.printf(">>>>>>>>>>>Found a new device asynchronously: %s\n", pDevice->toString().c_str());
+    } )
+    ) {
+    delay(BT_DISCOVER_TIME);
+    Serial.print("Stopping discoverAsync... ");
+    SerialBT.discoverAsyncStop();
+    Serial.println("discoverAsync stopped");
+    delay(5000);
+    if(btDeviceList->getCount() > 0) {
+      BTAddress addr;
+      int channel=0;
+      Serial.println("Found devices:");
+      for (int i=0; i < btDeviceList->getCount(); i++) {
+        BTAdvertisedDevice *device=btDeviceList->getDevice(i);
+        Serial.printf(" ----- %s  %s %d\n", device->getAddress().toString().c_str(), device->getName().c_str(), device->getRSSI());
+        std::map<int,std::string> channels=SerialBT.getChannels(device->getAddress());
+        Serial.printf("scanned for services, found %d\n", channels.size());
+        for(auto const &entry : channels) {
+          Serial.printf("     channel %d (%s)\n", entry.first, entry.second.c_str());
+        }
+        if(channels.size() > 0) {
+          addr = device->getAddress();
+          channel=channels.begin()->first;
+        }
+      }
+      if(addr) {
+        Serial.printf("connecting to %s - %d\n", addr.toString().c_str(), channel);
+        SerialBT.connect(addr, channel, sec_mask, role);
+      }
+    } else {
+      Serial.println("Didn't find any devices");
+    }
+  } else {
+    Serial.println("Error on discoverAsync f.e. not workin after a \"connect\"");
+  }
+}
+
+
+String sendData="Hi from esp32!\n";
+
+void loop() {
+  if(! SerialBT.isClosed() && SerialBT.connected()) {
+    if( SerialBT.write((const uint8_t*) sendData.c_str(),sendData.length()) != sendData.length()) {
+      Serial.println("tx: error");
+    } else {
+      Serial.printf("tx: %s",sendData.c_str());
+    }
+    if(SerialBT.available()) {
+      Serial.print("rx: ");
+      while(SerialBT.available()) {
+        int c=SerialBT.read();
+        if(c >= 0) {
+          Serial.print((char) c);
+        }
+      }
+      Serial.println();
+    }
+  } else {
+    Serial.println("not connected");
+  }
+  delay(1000);
+}

libraries/BluetoothSerial/src/BTAddress.cpp

@@ -29,6 +29,9 @@ BTAddress::BTAddress(esp_bd_addr_t address) {
 	memcpy(m_address, address, ESP_BD_ADDR_LEN);
 } // BTAddress
 
+BTAddress::BTAddress() {
+	bzero(m_address, ESP_BD_ADDR_LEN);
+} // BTAddress
 
 /**
  * @brief Create an address from a hex string
@@ -64,13 +67,20 @@ bool BTAddress::equals(BTAddress otherAddress) {
 	return memcmp(otherAddress.getNative(), m_address, 6) == 0;
 } // equals
 
+BTAddress::operator bool () const {
+	for(int i = 0; i < ESP_BD_ADDR_LEN; i++){
+		if(this->m_address[i])
+			return true;
+	}
+	return false;
+} // operator ()
 
 /**
  * @brief Return the native representation of the address.
  * @return The native representation of the address.
  */
-esp_bd_addr_t *BTAddress::getNative() {
-	return &m_address;
+esp_bd_addr_t *BTAddress::getNative() const {
+	return const_cast<esp_bd_addr_t *>(&m_address);
 } // getNative
 
 
@@ -85,7 +95,7 @@ esp_bd_addr_t *BTAddress::getNative() {
  *
  * @return The string representation of the address.
  */
-std::string BTAddress::toString() {
+std::string BTAddress::toString() const {
 	auto size = 18;
 	char *res = (char*)malloc(size);
 	snprintf(res, size, "%02x:%02x:%02x:%02x:%02x:%02x", m_address[0], m_address[1], m_address[2], m_address[3], m_address[4], m_address[5]);

libraries/BluetoothSerial/src/BTAddress.h

@@ -22,11 +22,14 @@
  */
 class BTAddress {
 public:
+	BTAddress();
 	BTAddress(esp_bd_addr_t address);
 	BTAddress(std::string stringAddress);
 	bool           equals(BTAddress otherAddress);
-	esp_bd_addr_t* getNative();
-	std::string    toString();
+	operator bool () const;
+
+	esp_bd_addr_t* getNative() const;
+	std::string    toString() const;
 
 private:
 	esp_bd_addr_t m_address;

libraries/BluetoothSerial/src/BTAdvertisedDevice.h

@@ -16,14 +16,14 @@ public:
     virtual ~BTAdvertisedDevice() = default;
 
     virtual BTAddress   getAddress();
-    virtual uint32_t    getCOD();
-    virtual std::string getName();
-    virtual int8_t      getRSSI();
+    virtual uint32_t    getCOD() const;
+    virtual std::string getName() const;
+    virtual int8_t      getRSSI() const;
 
 
-    virtual bool        haveCOD();
-    virtual bool        haveName();
-    virtual bool        haveRSSI();
+    virtual bool        haveCOD() const;
+    virtual bool        haveName() const;
+    virtual bool        haveRSSI() const;
 
     virtual std::string toString();
 };
@@ -35,14 +35,14 @@ public:
     
 
     BTAddress   getAddress();
-    uint32_t    getCOD();
-    std::string getName();
-    int8_t      getRSSI();
+    uint32_t    getCOD() const;
+    std::string getName() const;
+    int8_t      getRSSI() const;
 
 
-    bool        haveCOD();
-    bool        haveName();
-    bool        haveRSSI();
+    bool        haveCOD() const;
+    bool        haveName() const;
+    bool        haveRSSI() const;
 
     std::string toString();
 
@@ -62,4 +62,4 @@ public:
     int8_t      m_rssi;
 };
 
-#endif
\ No newline at end of file
+#endif

libraries/BluetoothSerial/src/BTAdvertisedDeviceSet.cpp

@@ -25,14 +25,14 @@ BTAdvertisedDeviceSet::BTAdvertisedDeviceSet() {
 } // BTAdvertisedDeviceSet
 
 BTAddress   BTAdvertisedDeviceSet::getAddress()    { return m_address; }
-uint32_t    BTAdvertisedDeviceSet::getCOD()        { return m_cod; }
-std::string BTAdvertisedDeviceSet::getName()       { return m_name; }
-int8_t      BTAdvertisedDeviceSet::getRSSI()	    { return m_rssi; }
+uint32_t    BTAdvertisedDeviceSet::getCOD() const       { return m_cod; }
+std::string BTAdvertisedDeviceSet::getName() const      { return m_name; }
+int8_t      BTAdvertisedDeviceSet::getRSSI() const	    { return m_rssi; }
 
 
-bool        BTAdvertisedDeviceSet::haveCOD()   { return m_haveCOD; }
-bool        BTAdvertisedDeviceSet::haveName()  { return m_haveName; }
-bool        BTAdvertisedDeviceSet::haveRSSI()  { return m_haveRSSI; }
+bool        BTAdvertisedDeviceSet::haveCOD() const  { return m_haveCOD; }
+bool        BTAdvertisedDeviceSet::haveName() const { return m_haveName; }
+bool        BTAdvertisedDeviceSet::haveRSSI() const { return m_haveRSSI; }
 
 /**
  * @brief Create a string representation of this device.

libraries/BluetoothSerial/src/BluetoothSerial.cpp

@@ -72,18 +72,30 @@ static esp_bt_pin_code_t _pin_code;
 static int _pin_len;
 static bool _isPinSet;
 static bool _enableSSP;
+static esp_spp_sec_t _sec_mask;
+static esp_spp_role_t _role;
+// start connect on ESP_SPP_DISCOVERY_COMP_EVT or save entry for getChannels
+static bool _doConnect;
+static std::map <int, std::string> sdpRecords;
 
 static BTScanResultsSet scanResults;
 static BTAdvertisedDeviceCb advertisedDeviceCb = nullptr;
 
+// _spp_event_group
 #define SPP_RUNNING     0x01
 #define SPP_CONNECTED   0x02
 #define SPP_CONGESTED   0x04
+// true until OPEN successful, changes to false on CLOSE
 #define SPP_DISCONNECTED 0x08
+// true until connect(), changes to true on CLOSE
+#define SPP_CLOSED      0x10
 
+// _bt_event_group
 #define BT_DISCOVERY_RUNNING    0x01
 #define BT_DISCOVERY_COMPLETED  0x02
 
+#define BT_SDP_RUNNING          0x04
+#define BT_SDP_COMPLETED        0x08
 
 typedef struct {
         size_t len;
@@ -98,7 +110,7 @@ static char *bda2str(esp_bd_addr_t bda, char *str, size_t size)
   }
 
   uint8_t *p = bda;
-  sprintf(str, "%02x:%02x:%02x:%02x:%02x:%02x",
+  snprintf(str, size, "%02x:%02x:%02x:%02x:%02x:%02x",
           p[0], p[1], p[2], p[3], p[4], p[5]);
   return str;
 }
@@ -280,13 +292,15 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
 
     case ESP_SPP_CLOSE_EVT://Client connection closed
         if ((param->close.async == false && param->close.status == ESP_SPP_SUCCESS) || param->close.async) {
-            log_i("ESP_SPP_CLOSE_EVT: %u", secondConnectionAttempt);
+            log_i("ESP_SPP_CLOSE_EVT status:%d handle:%d close_by_remote:%d attempt %u", param->close.status,
+                 param->close.handle, param->close.async, secondConnectionAttempt);
             if(secondConnectionAttempt) {
                 secondConnectionAttempt = false;
             } else {
                 _spp_client = 0;
                 xEventGroupSetBits(_spp_event_group, SPP_DISCONNECTED);
                 xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
+                xEventGroupSetBits(_spp_event_group, SPP_CLOSED);
             }        
             xEventGroupClearBits(_spp_event_group, SPP_CONNECTED);
         } else {
@@ -333,13 +347,37 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
         break;
 
     case ESP_SPP_DISCOVERY_COMP_EVT://discovery complete
-        log_i("ESP_SPP_DISCOVERY_COMP_EVT");
+        log_i("ESP_SPP_DISCOVERY_COMP_EVT num=%d", param->disc_comp.scn_num);
         if (param->disc_comp.status == ESP_SPP_SUCCESS) {
-            log_i("ESP_SPP_DISCOVERY_COMP_EVT: spp connect to remote");
-            esp_spp_connect(ESP_SPP_SEC_AUTHENTICATE, ESP_SPP_ROLE_MASTER, param->disc_comp.scn[0], _peer_bd_addr);
+            for(int i=0; i < param->disc_comp.scn_num; i++) {
+                log_d("ESP_SPP_DISCOVERY_COMP_EVT: spp [%d] channel: %d service name:%s", i, param->disc_comp.scn[i], param->disc_comp.service_name[0]);
+            }
+            if(_doConnect) {
+                if(param->disc_comp.scn_num > 0) {
+#if (ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_INFO)
+                    char bda_str[18];
+                    log_i("ESP_SPP_DISCOVERY_COMP_EVT: spp connect to remote %s channel %d",
+                        bda2str(_peer_bd_addr, bda_str, sizeof(bda_str)),
+                        param->disc_comp.scn[0]);
+#endif
+                    xEventGroupClearBits(_spp_event_group, SPP_CLOSED);
+                    if(esp_spp_connect(_sec_mask, _role, param->disc_comp.scn[0], _peer_bd_addr) != ESP_OK) {
+                        log_e("ESP_SPP_DISCOVERY_COMP_EVT connect failed");
+                        xEventGroupSetBits(_spp_event_group, SPP_CLOSED);
+                    }
+                } else {
+                    log_e("ESP_SPP_DISCOVERY_COMP_EVT remote doesn't offer an SPP channel");
+                    xEventGroupSetBits(_spp_event_group, SPP_CLOSED);
+                }
+            } else {
+                for(int i=0; i < param->disc_comp.scn_num; i++) {
+                    sdpRecords[param->disc_comp.scn[i]] = param->disc_comp.service_name[0];
+                }
+            }
         } else {
             log_e("ESP_SPP_DISCOVERY_COMP_EVT failed!, status:%d", param->disc_comp.status);
         }
+        xEventGroupSetBits(_bt_event_group, BT_SDP_COMPLETED);
         break;
 
     case ESP_SPP_OPEN_EVT://Client connection open
@@ -360,10 +398,15 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
         break;
 
     case ESP_SPP_CL_INIT_EVT://client initiated a connection
-        log_i("ESP_SPP_CL_INIT_EVT");
+        if (param->cl_init.status == ESP_SPP_SUCCESS) {
+            log_i("ESP_SPP_CL_INIT_EVT handle:%d sec_id:%d", param->cl_init.handle, param->cl_init.sec_id);
+        } else {
+            log_i("ESP_SPP_CL_INIT_EVT status:%d", param->cl_init.status);
+        }
         break;
 
     default:
+        log_i("ESP_SPP_* event unhandled %d", event);
         break;
     }
     if(custom_spp_callback)(*custom_spp_callback)(event, param);
@@ -377,7 +420,7 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
 {
     switch(event){
         case ESP_BT_GAP_DISC_RES_EVT: {
-            log_i("ESP_BT_GAP_DISC_RES_EVT");
+            log_i("ESP_BT_GAP_DISC_RES_EVT properties=%d", param->disc_res.num_prop);
 #if (ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_INFO)
             char bda_str[18];
             log_i("Scanned device: %s", bda2str(param->disc_res.bda, bda_str, 18));
@@ -417,28 +460,29 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
                         break;
 
                     case ESP_BT_GAP_DEV_PROP_COD:
-                        log_d("ESP_BT_GAP_DEV_PROP_COD");
                         if (param->disc_res.prop[i].len <= sizeof(int)) {
                             uint32_t cod = 0;
                             memcpy(&cod, param->disc_res.prop[i].val, param->disc_res.prop[i].len);
                             advertisedDevice.setCOD(cod);
+                            log_d("ESP_BT_GAP_DEV_PROP_COD 0x%x", cod);
                         } else {
-                            log_d("Value size larger than integer");
+                            log_d("ESP_BT_GAP_DEV_PROP_COD invalid COD: Value size larger than integer");
                         }
                         break;
 
                     case ESP_BT_GAP_DEV_PROP_RSSI:
-                        log_d("ESP_BT_GAP_DEV_PROP_RSSI");
                         if (param->disc_res.prop[i].len <= sizeof(int)) {
                             uint8_t rssi = 0;
                             memcpy(&rssi, param->disc_res.prop[i].val, param->disc_res.prop[i].len);
+                            log_d("ESP_BT_GAP_DEV_PROP_RSSI %d", rssi);
                             advertisedDevice.setRSSI(rssi);
                         } else {
-                            log_d("Value size larger than integer");
+                            log_d("ESP_BT_GAP_DEV_PROP_RSSI invalid RSSI: Value size larger than integer");
                         }
                         break;
                         
                     default:
+                        log_i("ESP_BT_GAP_DISC_RES_EVT unknown property [%d]:type:%d", i, param->disc_res.prop[i].type);
                         break;
                 }
                 if (_isRemoteAddressSet)
@@ -455,11 +499,12 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
         break;
 
         case ESP_BT_GAP_DISC_STATE_CHANGED_EVT:
-            log_i("ESP_BT_GAP_DISC_STATE_CHANGED_EVT");
             if (param->disc_st_chg.state == ESP_BT_GAP_DISCOVERY_STOPPED) {
+                log_i("ESP_BT_GAP_DISC_STATE_CHANGED_EVT stopped");
                 xEventGroupClearBits(_bt_event_group, BT_DISCOVERY_RUNNING);
                 xEventGroupSetBits(_bt_event_group, BT_DISCOVERY_COMPLETED);
             } else { // ESP_BT_GAP_DISCOVERY_STARTED
+                log_i("ESP_BT_GAP_DISC_STATE_CHANGED_EVT started");
                 xEventGroupClearBits(_bt_event_group, BT_DISCOVERY_COMPLETED);
                 xEventGroupSetBits(_bt_event_group, BT_DISCOVERY_RUNNING);
             }
@@ -522,7 +567,24 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
             log_i("ESP_BT_GAP_KEY_REQ_EVT Please enter passkey!");
             break;
 
+        case ESP_BT_GAP_CONFIG_EIR_DATA_EVT:
+            log_i("ESP_BT_GAP_CONFIG_EIR_DATA_EVT: stat:%d num:%d", param->config_eir_data.stat, param->config_eir_data.eir_type_num);
+            break;
+
+        case ESP_BT_GAP_READ_REMOTE_NAME_EVT:
+            if (param->read_rmt_name.stat == ESP_BT_STATUS_SUCCESS ) {
+                log_i("ESP_BT_GAP_READ_REMOTE_NAME_EVT: %s", param->read_rmt_name.rmt_name);
+            } else {
+                log_i("ESP_BT_GAP_READ_REMOTE_NAME_EVT: no success stat:%d", param->read_rmt_name.stat);
+            }
+            break;
+
+        case ESP_BT_GAP_MODE_CHG_EVT:
+            log_i("ESP_BT_GAP_MODE_CHG_EVT: mode: %d", param->mode_chg.mode);
+            break;
+
         default:
+            log_i("ESP-BT_GAP_* unknown message: %d", event);
             break;
     }
 }
@@ -546,6 +608,7 @@ static bool _init_bt(const char *deviceName)
         xEventGroupClearBits(_spp_event_group, 0xFFFFFF);
         xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
         xEventGroupSetBits(_spp_event_group, SPP_DISCONNECTED);
+        xEventGroupSetBits(_spp_event_group, SPP_CLOSED);
     }
     if (_spp_rx_queue == NULL){
         _spp_rx_queue = xQueueCreate(RX_QUEUE_SIZE, sizeof(uint8_t)); //initialize the queue
@@ -623,6 +686,7 @@ static bool _init_bt(const char *deviceName)
     esp_bt_dev_set_device_name(deviceName);
 
     if (_isPinSet) {
+        log_i("pin set");
         btSetPin();
     }
 
@@ -690,7 +754,16 @@ static bool _stop_bt()
 
 static bool waitForConnect(int timeout) {
     TickType_t xTicksToWait = timeout / portTICK_PERIOD_MS;
-    return (xEventGroupWaitBits(_spp_event_group, SPP_CONNECTED, pdFALSE, pdTRUE, xTicksToWait) & SPP_CONNECTED) != 0;
+    // wait for connected or closed
+    EventBits_t rc = xEventGroupWaitBits(_spp_event_group, SPP_CONNECTED | SPP_CLOSED, pdFALSE, pdFALSE, xTicksToWait);
+    if((rc & SPP_CONNECTED) != 0)
+        return true;
+    else if((rc & SPP_CLOSED) != 0) {
+        log_d("connection closed!");
+        return false;
+    }
+    log_d("timeout");
+    return false;
 }
 
 static bool waitForDiscovered(int timeout) {
@@ -698,6 +771,11 @@ static bool waitForDiscovered(int timeout) {
     return (xEventGroupWaitBits(_spp_event_group, BT_DISCOVERY_COMPLETED, pdFALSE, pdTRUE, xTicksToWait) & BT_DISCOVERY_COMPLETED) != 0;
 }
 
+static bool waitForSDPRecord(int timeout) {
+    TickType_t xTicksToWait = timeout / portTICK_PERIOD_MS;
+    return (xEventGroupWaitBits(_bt_event_group, BT_SDP_COMPLETED, pdFALSE, pdTRUE, xTicksToWait) & BT_SDP_COMPLETED) != 0;
+}
+
 /*
  * Serial Bluetooth Arduino
  *
@@ -713,6 +791,9 @@ BluetoothSerial::~BluetoothSerial(void)
     _stop_bt();
 }
 
+/**
+ * @Param isMaster set to true if you want to connect to an other device
+ */
 bool BluetoothSerial::begin(String localName, bool isMaster)
 {
     _isMaster = isMaster;
@@ -733,7 +814,7 @@ int BluetoothSerial::available(void)
 int BluetoothSerial::peek(void)
 {
     uint8_t c;
-    if (_spp_rx_queue && xQueuePeek(_spp_rx_queue, &c, 0)){
+    if (_spp_rx_queue && xQueuePeek(_spp_rx_queue, &c, this->timeoutTicks)){
         return c;
     }
     return -1;
@@ -744,16 +825,24 @@ bool BluetoothSerial::hasClient(void)
     return _spp_client > 0;
 }
 
-int BluetoothSerial::read(void)
+int BluetoothSerial::read()
 {
 
     uint8_t c = 0;
-    if (_spp_rx_queue && xQueueReceive(_spp_rx_queue, &c, 0)){
+    if (_spp_rx_queue && xQueueReceive(_spp_rx_queue, &c, this->timeoutTicks)){
         return c;
     }
     return -1;
 }
 
+/**
+ * Set timeout for read / peek
+ */
+void BluetoothSerial::setTimeout(int timeoutMS)
+{
+    this->timeoutTicks=timeoutMS / portTICK_PERIOD_MS;
+}
+
 size_t BluetoothSerial::write(uint8_t c)
 {
     return write(&c, 1);
@@ -812,6 +901,7 @@ void BluetoothSerial::enableSSP() {
      * Use fixed pin code
 */
 bool BluetoothSerial::setPin(const char *pin) {
+    log_i("pin: %s", pin);
     bool isEmpty =  !(pin  && *pin);
     if (isEmpty && !_isPinSet) {
         return true; // nothing to do
@@ -837,7 +927,10 @@ bool BluetoothSerial::connect(String remoteName)
         return false; 
     }
     disconnect();
+    _doConnect = true;
     _isRemoteAddressSet = false;
+	_sec_mask = ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE;
+	_role = ESP_SPP_ROLE_MASTER;
     strncpy(_remote_name, remoteName.c_str(), ESP_BT_GAP_MAX_BDNAME_LEN);
     _remote_name[ESP_BT_GAP_MAX_BDNAME_LEN] = 0;
     log_i("master : remoteName");
@@ -847,13 +940,23 @@ bool BluetoothSerial::connect(String remoteName)
 #else
         esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
 #endif
+    xEventGroupClearBits(_spp_event_group, SPP_CLOSED);
     if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, INQ_LEN, INQ_NUM_RSPS) == ESP_OK) {
         return waitForConnect(SCAN_TIMEOUT);
     }
     return false;
 }
 
-bool BluetoothSerial::connect(uint8_t remoteAddress[])
+/**
+ * @Param channel: specify channel or 0 for auto-detect
+ * @Param sec_mask:
+ *           ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE
+ *           ESP_SPP_SEC_NONE
+ * @Param role:
+ *           ESP_SPP_ROLE_MASTER   master can handle up to 7 connections to slaves
+ *           ESP_SPP_ROLE_SLAVE    can only have one connection to a master
+ */
+bool BluetoothSerial::connect(uint8_t remoteAddress[], int channel, esp_spp_sec_t sec_mask, esp_spp_role_t role)
 {
     if (!isReady(true, READY_TIMEOUT)) return false;
     if (!remoteAddress) {
@@ -861,10 +964,35 @@ bool BluetoothSerial::connect(uint8_t remoteAddress[])
         return false; 
     }
     disconnect();
+    _doConnect = true;
     _remote_name[0] = 0;
     _isRemoteAddressSet = true;
+	_sec_mask = sec_mask;
+	_role = role;
     memcpy(_peer_bd_addr, remoteAddress, ESP_BD_ADDR_LEN);
     log_i("master : remoteAddress");
+    xEventGroupClearBits(_spp_event_group, SPP_CLOSED);
+    if (channel > 0) {
+        char bda_str[18];
+        log_i("spp connect to remote %s channel %d",
+            bda2str(_peer_bd_addr, bda_str, sizeof(bda_str)),
+            channel);
+        if(esp_spp_connect(sec_mask, role, channel, _peer_bd_addr) != ESP_OK ) {
+			log_e("spp connect failed");
+			return false;
+		}
+        bool rc=waitForConnect(READY_TIMEOUT);
+        if(rc) {
+            log_i("connected");
+        } else {
+            if(this->isClosed()) {
+                log_e("connect failed");
+            } else {
+                log_e("connect timed out after %dms", READY_TIMEOUT);
+            }
+        }
+        return rc;
+    }
     if (esp_spp_start_discovery(_peer_bd_addr) == ESP_OK) {
         return waitForConnect(READY_TIMEOUT);
     }
@@ -874,6 +1002,7 @@ bool BluetoothSerial::connect(uint8_t remoteAddress[])
 bool BluetoothSerial::connect()
 {
     if (!isReady(true, READY_TIMEOUT)) return false;
+    _doConnect = true;
     if (_isRemoteAddressSet){
         disconnect();
         // use resolved or set address first
@@ -924,6 +1053,13 @@ bool BluetoothSerial::connected(int timeout) {
     return waitForConnect(timeout);
 }
 
+/**
+ * true if a connection terminated or a connection attempt failed
+ */
+bool BluetoothSerial::isClosed() {
+    return xEventGroupGetBits(_spp_event_group) & SPP_CLOSED;
+}
+
 bool BluetoothSerial::isReady(bool checkMaster, int timeout) {
     if (checkMaster && !_isMaster) {
         log_e("Master mode is not active. Call begin(localName, true) to enable Master mode");
@@ -956,6 +1092,7 @@ BTScanResults* BluetoothSerial::discover(int timeoutMs) {
     esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
     if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, timeout, 0) == ESP_OK) {
         waitForDiscovered(timeoutMs);
+        log_i("gap_cancel_discovery()");
         esp_bt_gap_cancel_discovery();
     }
     return &scanResults;
@@ -1008,4 +1145,31 @@ BluetoothSerial::operator bool() const
 {
     return true;
 }
+
+/**
+ * SDP scan address
+ * esp_spp_start_discovery doesn't tell us the btAddress in the callback, so we have to wait until it's finished
+ */
+std::map<int, std::string> BluetoothSerial::getChannels(const BTAddress &remoteAddress) {
+    if(xEventGroupGetBits(_bt_event_group) & BT_SDP_RUNNING) {
+        log_e("getChannels failed - already running");
+    }
+    xEventGroupSetBits(_bt_event_group, BT_SDP_RUNNING);
+    xEventGroupClearBits(_bt_event_group, BT_SDP_COMPLETED);
+    _doConnect = false;
+    sdpRecords.clear();
+    log_d("esp_spp_start_discovery");
+    if (esp_spp_start_discovery(*remoteAddress.getNative()) != ESP_OK) {
+        log_e("esp_spp_start_discovery failed");
+    } else {
+        if(! waitForSDPRecord(READY_TIMEOUT)) {
+            log_e("getChannels failed timeout");
+        }
+        log_d("esp_spp_start_discovery wait for BT_SDP_COMPLETED done (%dms)", READY_TIMEOUT);
+    }
+    log_d("esp_spp_start_discovery done, found %d services", sdpRecords.size());
+    xEventGroupClearBits(_bt_event_group, BT_SDP_RUNNING);
+    return sdpRecords;
+}
+
 #endif

libraries/BluetoothSerial/src/BluetoothSerial.h

@@ -24,6 +24,7 @@
 #include <esp_gap_bt_api.h>
 #include <esp_spp_api.h>
 #include <functional>
+#include <map>
 #include "BTScan.h"
 
 typedef std::function<void(const uint8_t *buffer, size_t size)> BluetoothSerialDataCb;
@@ -50,6 +51,7 @@ class BluetoothSerial: public Stream
         size_t write(const uint8_t *buffer, size_t size);
         void flush();
         void end(void);
+        void setTimeout(int timeoutMS);
         void onData(BluetoothSerialDataCb cb);
         esp_err_t register_callback(esp_spp_cb_t * callback);
         
@@ -60,9 +62,12 @@ class BluetoothSerial: public Stream
         void enableSSP();
         bool setPin(const char *pin);
         bool connect(String remoteName);
-        bool connect(uint8_t remoteAddress[]);
+        bool connect(uint8_t remoteAddress[], int channel=0, esp_spp_sec_t sec_mask=(ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE), esp_spp_role_t role=ESP_SPP_ROLE_MASTER);
+        bool connect(const BTAddress &remoteAddress, int channel=0, esp_spp_sec_t sec_mask=(ESP_SPP_SEC_ENCRYPT|ESP_SPP_SEC_AUTHENTICATE), esp_spp_role_t role=ESP_SPP_ROLE_MASTER) {
+			return connect(*remoteAddress.getNative(), channel, sec_mask); };
         bool connect();
         bool connected(int timeout=0);
+        bool isClosed();
         bool isReady(bool checkMaster=false, int timeout=0);
         bool disconnect();
         bool unpairDevice(uint8_t remoteAddress[]);
@@ -73,6 +78,8 @@ class BluetoothSerial: public Stream
         void discoverClear();
         BTScanResults* getScanResults();
         
+        std::map<int, std::string> getChannels(const BTAddress &remoteAddress);
+
         const int INQ_TIME = 1280;   // Inquire Time unit 1280 ms
         const int MIN_INQ_TIME = (ESP_BT_GAP_MIN_INQ_LEN * INQ_TIME);
         const int MAX_INQ_TIME = (ESP_BT_GAP_MAX_INQ_LEN * INQ_TIME);
@@ -80,7 +87,7 @@ class BluetoothSerial: public Stream
         operator bool() const;
     private:
         String local_name;
-
+        int timeoutTicks=0;
 };
 
 #endif