holy smokes! thats a lot of BLE improvements!

include/baros_ble.h

@@ -3,6 +3,18 @@
 
 #include <stdbool.h>
 #include "esp_event_base.h"
+#include "host/ble_hs.h"
+
+
+#ifdef MACSTR
+  #undef MACSTR
+  #define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
+#endif
+
+#ifdef MAC2STR
+  #undef MAC2STR
+  #define MAC2STR(a) (uint8_t)(a)[0], (uint8_t)(a)[1], (uint8_t)(a)[2], (uint8_t)(a)[3], (uint8_t)(a)[4], (uint8_t)(a)[5]
+#endif
 
 #define BAROS_NAME_MAX_LEN                    (    32)
 #define BAROS_BLE_SERVICE_PUMP_ENABLED        (0x4200)
@@ -32,11 +44,17 @@ typedef struct {
 typedef struct {
   char *name;
   uint8_t name_len;
-  uint8_t mac[6];
+  ble_addr_t addr;
   baros_role_t role;
 } baros_ble_discovery_t;
 
-uint8_t baros_ble_init(baros_ble_cfg_t *cfg);
+typedef void baros_ble_connect_callback_t(ble_addr_t addr, void *arg);
+
+typedef struct {
+  ble_addr_t addr;
+  void *arg;
+  baros_ble_connect_callback_t *cb;
+} baros_ble_connect_cfg_t;
 
 typedef struct baros_list_node baros_list_node_t;
 
@@ -48,10 +66,17 @@ struct baros_list_node {
   void *element;
 };
 
-typedef bool baros_list_find_element_callback(void *element);
+uint8_t baros_ble_init(baros_ble_cfg_t *cfg);
+void baros_ble_connect(baros_ble_connect_cfg_t *cfg);
 
-void *baros_list_find_element(baros_list_node_t *list, baros_list_find_element_callback *cb);
+typedef bool baros_list_find_element_callback(void *element, void *arg);
-void *baros_list_find_node(baros_list_node_t *list, baros_list_find_element_callback *cb);
+
+void *baros_list_find_element(baros_list_node_t *list, baros_list_find_element_callback *cb, void *arg);
+void *baros_list_find_node(baros_list_node_t *list, baros_list_find_element_callback *cb, void *arg);
+void baros_list_free(baros_list_node_t *list);
+baros_list_node_t *baros_list_find_all_nodes(baros_list_node_t *list, baros_list_find_element_callback *cb, void *arg);
 baros_list_node_t *baros_list_add(baros_list_node_t *list, void *element);
 
+extern baros_list_node_t *peer_list;
+
 #endif

src/baros.c

@@ -2,6 +2,9 @@
 #include <stdlib.h>
 #include "baros.h"
 #include "baros_ble.h"
+#include "esp_log.h"
+
+#define TAG "BAROS"
 
 uint8_t baros_init(baros_cfg_t *cfg) {
   baros_ble_cfg_t ble_cfg = {
@@ -16,17 +19,28 @@ uint8_t baros_init(baros_cfg_t *cfg) {
 
 baros_list_node_t *baros_list_add(baros_list_node_t *list, void *element) {
   baros_list_node_t *node = malloc(sizeof(baros_list_node_t));
+
+  if(!node) {
+    ESP_LOGE(TAG, "unable to allocate memory for new node");
+
+    return NULL;
+  }
+
   bzero(node, sizeof(baros_list_node_t));
 
   node->element = element;
 
   if(list == NULL) {
+    ESP_LOGD(TAG, "Creating new list");
+
     node->first = node;
     node->last = node;
 
     return node;
   }
 
+  ESP_LOGD(TAG, "Adding new element");
+
   baros_list_node_t *last = list->last;
 
   node->first = list->first;
@@ -44,20 +58,52 @@ baros_list_node_t *baros_list_add(baros_list_node_t *list, void *element) {
   return node;
 }
 
-void *baros_list_find_node(baros_list_node_t *list, baros_list_find_element_callback *cb) {
+void baros_list_free(baros_list_node_t *list) {
+  baros_list_node_t *last = list->last;
+  baros_list_node_t *prev;
+
+  while(last) {
+    prev = last->prev;
+    free(last);
+    last = prev;
+  }
+}
+
+baros_list_node_t *baros_list_find_all_nodes(baros_list_node_t *list, baros_list_find_element_callback *cb, void *arg) {
+  baros_list_node_t *ret = NULL;
   baros_list_node_t *n = list->first;
 
   while(n) {
-    if(cb(n->element)) {
+    if(cb(n->element, arg)) {
+      if(ret == NULL) {
+        ret = baros_list_add(NULL, n->element);
+      } else {
+        baros_list_add(ret, n->element);
+      }
+    }
+
+    n = n->next;
+  }
+
+  return ret;
+}
+
+void *baros_list_find_node(baros_list_node_t *list, baros_list_find_element_callback *cb, void *arg) {
+  baros_list_node_t *n = list->first;
+
+  while(n) {
+    if(cb(n->element, arg)) {
       return n;
     }
+
+    n = n->next;
   }
 
   return NULL;
 }
 
-void *baros_list_find_element(baros_list_node_t *list, baros_list_find_element_callback *cb) {
+void *baros_list_find_element(baros_list_node_t *list, baros_list_find_element_callback *cb, void *arg) {
-  baros_list_node_t *n = baros_list_find_node(list, cb);
+  baros_list_node_t *n = baros_list_find_node(list, cb, arg);
 
   if(n) {
     return n->element;

src/baros_ble.c

@@ -42,6 +42,7 @@ static int svc_access_system(uint16_t conn_handle, uint16_t attr_handle, struct
 static int barback_ble_char_access(uint16_t conn_handle, uint16_t attr_handle, struct ble_gatt_access_ctxt *ctxt, void *arg);
 
 baros_list_node_t *peer_list = NULL;
+baros_ble_connect_cfg_t *connect_attempt = NULL;
 
 static const struct ble_gatt_svc_def service_defs[] = {
   {
@@ -252,14 +253,14 @@ static int ble_gap_event(struct ble_gap_event *event, void *arg) {
 
         baros_role_t role = fields.svc_data_uuid16[2];
 
-        ESP_LOGD(TAG, "Discovery: %s", name);
+        ESP_LOGI(TAG, "Discovery [" MACSTR "]: %s", MAC2STR(event->disc.addr.val), name);
 
         baros_ble_discovery_t *discovery = malloc(sizeof(baros_ble_discovery_t));
 
         discovery->name_len = fields.name_len;
         discovery->name = name;
         discovery->role = role;
-        memcpy(discovery->mac, &event->disc.addr.val, 6);
+        discovery->addr = event->disc.addr;
 
         if(peer_list == NULL) {
           peer_list = baros_list_add(NULL, discovery);
@@ -285,6 +286,11 @@ static int ble_gap_event(struct ble_gap_event *event, void *arg) {
       // always keep advertising
       ble_advertise();
 
+      if(connect_attempt != NULL && connect_attempt->cb != NULL) {
+        ESP_LOGI(TAG, "addr: %p arg: %p", ((baros_ble_discovery_t*)connect_attempt->arg)->addr, connect_attempt->arg);
+        connect_attempt->cb(((baros_ble_discovery_t*)connect_attempt->arg)->addr, connect_attempt->arg);
+      }
+
       break;
 
     case BLE_GAP_EVENT_DISCONNECT:
@@ -401,8 +407,17 @@ void nimble_host_task(void *param) {
   nimble_port_run();
 }
 
-uint8_t baros_ble_scan(baros_role_t role) {
+void baros_ble_connect(baros_ble_connect_cfg_t *cfg) {
-  return 0;
+  uint8_t addr_type;
+  connect_attempt = cfg;
+
+  ESP_LOGI(TAG, "Connect attempt to: " MACSTR, MAC2STR(cfg->addr.val));
+
+  ble_gap_disc_cancel();
+
+  ESP_ERROR_CHECK(ble_hs_id_infer_auto(0, &addr_type));
+  esp_err_t err = ble_gap_connect(addr_type, &cfg->addr, 30000, NULL, ble_gap_event, NULL);
+  ESP_LOGI(TAG, "err: 0x%x", err);
 }
 
 uint8_t baros_ble_init(baros_ble_cfg_t *cfg) {