first commit

components/nimble_tracker/__init__.py

@@ -0,0 +1,110 @@
+import esphome.codegen as cg
+import esphome.config_validation as cv
+from esphome.components.esp32 import add_idf_sdkconfig_option
+from esphome.const import (
+    CONF_ACTIVE,
+    CONF_ID,
+    CONF_INTERVAL,
+    CONF_DURATION,
+)
+
+DEPENDENCIES = ["esp32"]
+
+CONF_NIMBLE_ID = "nimble_ble_id"
+CONF_SCAN_PARAMETERS = "scan_parameters"
+
+CONF_WINDOW = "window"
+CONF_CONTINUOUS = "continuous"
+
+nimble_tracker_ns = cg.esphome_ns.namespace("nimble_tracker")
+
+NimbleTracker = nimble_tracker_ns.class_("NimbleTracker", cg.Component)
+NimbleDeviceListener = nimble_tracker_ns.class_("NimbleDeviceListener", cg.Component)
+
+
+def validate_scan_parameters(config):
+    duration = config[CONF_DURATION]
+    interval = config[CONF_INTERVAL]
+    window = config[CONF_WINDOW]
+
+    if window > interval:
+        raise cv.Invalid(
+            f"Scan window ({window}) needs to be smaller than scan interval ({interval})"
+        )
+
+    if interval.total_milliseconds * 3 > duration.total_milliseconds:
+        raise cv.Invalid(
+            "Scan duration needs to be at least three times the scan interval to"
+            "cover all BLE channels."
+        )
+
+    return config
+
+
+CONFIG_SCHEMA = cv.Schema(
+    {
+        cv.GenerateID(): cv.declare_id(NimbleTracker),
+        cv.Optional(CONF_SCAN_PARAMETERS, default={}): cv.All(
+            cv.Schema(
+                {
+                    cv.Optional(
+                        CONF_DURATION, default="5min"
+                    ): cv.positive_time_period_seconds,
+                    cv.Optional(
+                        CONF_INTERVAL, default="320ms"
+                    ): cv.positive_time_period_milliseconds,
+                    cv.Optional(
+                        CONF_WINDOW, default="30ms"
+                    ): cv.positive_time_period_milliseconds,
+                    cv.Optional(CONF_ACTIVE, default=True): cv.boolean,
+                    cv.Optional(CONF_CONTINUOUS, default=True): cv.boolean,
+                    cv.Optional(CONF_CONTINUOUS, default=True): cv.boolean,
+                }
+            ),
+            validate_scan_parameters,
+        ),
+    }
+).extend(cv.COMPONENT_SCHEMA)
+
+CONF_IRK = "irk"
+
+NIMBLE_DEVICE_LISTENER_SCHEMA = cv.Schema(
+    {
+        cv.GenerateID(CONF_NIMBLE_ID): cv.use_id(NimbleTracker),
+        cv.Optional(CONF_IRK): cv.string,
+    },
+    cv.has_exactly_one_key(CONF_IRK),
+)
+
+
+async def to_code(config):
+    # this initializes the component in the generated code
+    var = cg.new_Pvariable(config[CONF_ID])
+    await cg.register_component(var, config)
+
+    params = config[CONF_SCAN_PARAMETERS]
+    cg.add(var.set_scan_duration(params[CONF_DURATION]))
+    cg.add(var.set_scan_interval(int(params[CONF_INTERVAL].total_milliseconds / 0.625)))
+    cg.add(var.set_scan_window(int(params[CONF_WINDOW].total_milliseconds / 0.625)))
+    cg.add(var.set_scan_active(params[CONF_ACTIVE]))
+    cg.add(var.set_scan_continuous(params[CONF_CONTINUOUS]))
+
+    add_idf_sdkconfig_option("CONFIG_BT_ENABLED", True)
+    add_idf_sdkconfig_option("CONFIG_BT_BLUEDROID_ENABLED", False)
+    add_idf_sdkconfig_option("CONFIG_BT_NIMBLE_ENABLED", True)
+    add_idf_sdkconfig_option("CONFIG_MBEDTLS_HARDWARE_AES", False)
+
+    cg.add_library(
+        "esp-nimble-cpp=https://github.com/h2zero/esp-nimble-cpp.git#v1.4.1", None
+    )
+
+
+async def register_ble_device(var, config):
+    paren = await cg.get_variable(config[CONF_NIMBLE_ID])
+    cg.add(paren.register_listener(var))
+    return var
+
+
+async def device_listener_to_code(var, config):
+    if CONF_IRK in config:
+        cg.add(var.set_irk(config[CONF_IRK]))

components/nimble_tracker/irk_utils.cpp

@@ -0,0 +1,61 @@
+// Copied from https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/BleFingerprint.cpp
+#include "irk_utils.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        int bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data)
+        {
+            mbedtls_aes_context s = {0};
+            mbedtls_aes_init(&s);
+
+            if (mbedtls_aes_setkey_enc(&s, key, 128) != 0)
+            {
+                mbedtls_aes_free(&s);
+                return BLE_HS_EUNKNOWN;
+            }
+
+            if (mbedtls_aes_crypt_ecb(&s, MBEDTLS_AES_ENCRYPT, plaintext, enc_data) != 0)
+            {
+                mbedtls_aes_free(&s);
+                return BLE_HS_EUNKNOWN;
+            }
+
+            mbedtls_aes_free(&s);
+            return 0;
+        }
+
+        bool ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk)
+        {
+            struct encryption_block ecb;
+
+            auto irk32 = (const uint32_t *)irk;
+            auto key32 = (uint32_t *)&ecb.key[0];
+            auto pt32 = (uint32_t *)&ecb.plain_text[0];
+
+            key32[0] = irk32[0];
+            key32[1] = irk32[1];
+            key32[2] = irk32[2];
+            key32[3] = irk32[3];
+
+            pt32[0] = 0;
+            pt32[1] = 0;
+            pt32[2] = 0;
+            pt32[3] = 0;
+
+            ecb.plain_text[15] = rpa[3];
+            ecb.plain_text[14] = rpa[4];
+            ecb.plain_text[13] = rpa[5];
+
+            auto err = bt_encrypt_be(ecb.key, ecb.plain_text, ecb.cipher_text);
+
+            if (ecb.cipher_text[15] != rpa[0] || ecb.cipher_text[14] != rpa[1] || ecb.cipher_text[13] != rpa[2])
+                return false;
+
+            return true;
+        }
+
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/irk_utils.h

@@ -0,0 +1,24 @@
+// Copied from https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/BleFingerprint.cpp
+#pragma once
+
+#include "mbedtls/aes.h"
+#include "NimBLEDevice.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        int bt_encrypt_be(const uint8_t *key, const uint8_t *plaintext, uint8_t *enc_data);
+
+        struct encryption_block
+        {
+            uint8_t key[16];
+            uint8_t plain_text[16];
+            uint8_t cipher_text[16];
+        };
+
+        bool ble_ll_resolv_rpa(const uint8_t *rpa, const uint8_t *irk);
+
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_device_listener.cpp

@@ -0,0 +1,44 @@
+#include "esphome/core/log.h"
+#include "nimble_device_listener.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        static const char *const TAG = "nimble_device_listener";
+
+        void NimbleDeviceListener::set_irk(std::string irk_hex)
+        {
+            this->match_by_ = MATCH_BY_IRK;
+            this->irk_ = new uint8_t[16];
+
+            if (!hextostr(irk_hex.c_str(), this->irk_, 16))
+            {
+                // TODO: this logic should be moved to Python validation
+                ESP_LOGE(TAG, "Something is wrong with the irk!");
+            }
+        }
+
+        bool NimbleDeviceListener::parse_event(NimbleTrackerEvent *tracker_event)
+        {
+            if (tracker_event->getAddressType() != BLE_ADDR_RANDOM)
+            {
+                return false;
+            }
+
+            auto address = tracker_event->getAddress();
+            auto naddress = address.getNative();
+
+            if (ble_ll_resolv_rpa(naddress, this->irk_))
+            {
+                ESP_LOGD(TAG, "Found device %s", tracker_event->toString().c_str());
+                return this->update_state(tracker_event);
+            }
+            else
+            {
+                return false;
+            }
+        };
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_device_listener.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include "string_utils.h"
+#include "irk_utils.h"
+#include "nimble_tracker_event.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        class NimbleDeviceListener
+        {
+
+        public:
+            bool parse_event(NimbleTrackerEvent *tracker_event);
+            void set_irk(std::string irk_hex);
+
+        protected:
+            virtual bool update_state(NimbleTrackerEvent *tracker_event) = 0;
+
+            enum MatchType
+            {
+                MATCH_BY_IRK,
+            };
+            MatchType match_by_;
+
+            uint8_t *irk_;
+        };
+
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_tracker.cpp

@@ -0,0 +1,93 @@
+#include "nimble_tracker.h"
+#include "esphome/core/log.h"
+#include "esphome/core/hal.h"
+
+// using namespace esphome;
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        static const char *const TAG = "nimble_tracker";
+
+        class MyAdvertisedDeviceCallbacks : public BLEAdvertisedDeviceCallbacks
+        {
+        public:
+            MyAdvertisedDeviceCallbacks(NimbleTracker *nimble_tracker)
+            {
+                nimble_tracker_ = nimble_tracker;
+            }
+
+            void onResult(BLEAdvertisedDevice *advertised_device)
+            {
+                // Because setMaxResults is set to 0 for the NimBLEScan, we need to make a copy
+                // of the data of the advertised device, because this is deleted immediately by NimBLESCan
+                // after this callback is called.
+                auto *tracker_event = new NimbleTrackerEvent(
+                    advertised_device->getAddress(),
+                    advertised_device->getAddressType(),
+                    advertised_device->getRSSI(),
+                    advertised_device->getTXPower());
+
+                nimble_tracker_->tracker_events_.push(tracker_event);
+            }
+
+        protected:
+            NimbleTracker *nimble_tracker_;
+        };
+
+        void NimbleTracker::setup()
+        {
+            // Set the name to empty string to not broadcast the name
+            NimBLEDevice::init("");
+            this->pBLEScan_ = NimBLEDevice::getScan();
+            this->pBLEScan_->setInterval(this->scan_interval_);
+            this->pBLEScan_->setWindow(this->scan_window_);
+            this->pBLEScan_->setAdvertisedDeviceCallbacks(new MyAdvertisedDeviceCallbacks(this), true);
+            this->pBLEScan_->setActiveScan(this->scan_active_);
+            this->pBLEScan_->setDuplicateFilter(false);
+            this->pBLEScan_->setMaxResults(0);
+
+            ESP_LOGV(TAG, "Trying to start the scan");
+
+            if (!pBLEScan_->start(0, nullptr, false))
+            {
+                ESP_LOGE(TAG, "Error starting continuous ble scan");
+                // this->mark_failed();
+                return;
+            }
+
+            // TODO: It takes some time to setup bluetooth? Why not just move this to loop?
+            delay(200);
+        }
+
+        void NimbleTracker::loop()
+        {
+            if (!this->pBLEScan_->isScanning())
+            {
+                if (!this->pBLEScan_->start(0, nullptr, false))
+                {
+                    ESP_LOGE(TAG, "Error starting continuous ble scan");
+                    return;
+                }
+
+                // TODO: we shouldn't block the main thread here, instead work with a setTimeout callback?
+                delay(200);
+            }
+
+            NimbleTrackerEvent *tracker_event = this->tracker_events_.pop();
+
+            while (tracker_event != nullptr)
+            {
+                for (NimbleDeviceListener *listener : this->listeners_)
+                {
+                    listener->parse_event(tracker_event);
+                }
+
+                delete tracker_event;
+                tracker_event = this->tracker_events_.pop();
+            }
+        };
+
+    } // namespace esp32_ble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_tracker.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "esphome/core/component.h"
+#include "queue.h"
+#include "NimBLEDevice.h"
+#include "NimBLEAdvertisedDevice.h"
+#include "nimble_device_listener.h"
+#include "nimble_tracker_event.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        class NimbleTracker : public Component
+        {
+
+        public:
+            void set_scan_duration(uint32_t scan_duration) { scan_duration_ = scan_duration; }
+            void set_scan_interval(uint32_t scan_interval) { scan_interval_ = scan_interval; }
+            void set_scan_window(uint32_t scan_window) { scan_window_ = scan_window; }
+            void set_scan_active(bool scan_active) { scan_active_ = scan_active; }
+            void set_scan_continuous(bool scan_continuous) { scan_continuous_ = scan_continuous; }
+            Queue<NimbleTrackerEvent> tracker_events_;
+
+            void setup() override;
+            void loop() override;
+
+            void register_listener(NimbleDeviceListener *listener) { listeners_.push_back(listener); }
+
+        protected:
+            uint32_t scan_duration_;
+            uint32_t scan_interval_;
+            uint32_t scan_window_;
+            bool scan_active_;
+            bool scan_continuous_;
+            NimBLEScan *pBLEScan_;
+            std::vector<NimbleDeviceListener *> listeners_;
+        };
+    } // namespace esp32_ble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_tracker_event.cpp

@@ -0,0 +1,45 @@
+#include "nimble_tracker_event.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        NimbleTrackerEvent::NimbleTrackerEvent(NimBLEAddress address, uint8_t address_type, int rssi, int8_t tx_power)
+        {
+            this->address_ = address;
+            this->address_type_ = address_type;
+            this->rssi_ = rssi;
+            this->tx_power_ = tx_power;
+        }
+
+        int8_t NimbleTrackerEvent::getTXPower()
+        {
+            return this->tx_power_;
+        }
+
+        int NimbleTrackerEvent::getRSSI()
+        {
+            return this->rssi_;
+        }
+
+        uint8_t NimbleTrackerEvent::getAddressType()
+        {
+            return this->address_type_;
+        }
+
+        NimBLEAddress NimbleTrackerEvent::getAddress()
+        {
+            return this->address_;
+        }
+
+        std::string NimbleTrackerEvent::toString()
+        {
+            std::string result = "Address: " + this->address_.toString();
+            result += " Address type: " + std::to_string(this->address_type_);
+            result += " RSSI: " + std::to_string(this->rssi_);
+            result += " TX Power: " + std::to_string(this->tx_power_);
+            return result;
+        }
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/nimble_tracker_event.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "NimBLEDevice.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+        class NimbleTrackerEvent
+        {
+        public:
+            NimbleTrackerEvent(NimBLEAddress address, uint8_t address_type, int rssi, int8_t tx_power);
+            int8_t getTXPower();
+            int getRSSI();
+            uint8_t getAddressType();
+            NimBLEAddress getAddress();
+            std::string toString();
+
+        protected:
+            int8_t tx_power_;
+            int rssi_;
+            uint8_t address_type_;
+            NimBLEAddress address_;
+        };
+
+    } // namespace nimble_tracker
+
+} // namespace esphome

components/nimble_tracker/queue.h

@@ -0,0 +1,65 @@
+// Copied from https://github.com/esphome/esphome/blob/dev/esphome/components/esp32_ble_tracker/queue.h
+#pragma once
+
+#include "esphome/core/component.h"
+#include "esphome/core/helpers.h"
+
+#include <queue>
+#include <mutex>
+#include <cstring>
+
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+
+/*
+ * BLE events come in from a separate Task (thread) in the ESP32 stack. Rather
+ * than trying to deal with various locking strategies, all incoming GAP and GATT
+ * events will simply be placed on a semaphore guarded queue. The next time the
+ * component runs loop(), these events are popped off the queue and handed at
+ * this safer time.
+ */
+
+namespace esphome
+{
+  namespace nimble_tracker
+  {
+
+    template <class T>
+    class Queue
+    {
+    public:
+      Queue() { m_ = xSemaphoreCreateMutex(); }
+
+      void push(T *element)
+      {
+        if (element == nullptr)
+          return;
+        if (xSemaphoreTake(m_, 5L / portTICK_PERIOD_MS))
+        {
+          q_.push(element);
+          xSemaphoreGive(m_);
+        }
+      }
+
+      T *pop()
+      {
+        T *element = nullptr;
+
+        if (xSemaphoreTake(m_, 5L / portTICK_PERIOD_MS))
+        {
+          if (!q_.empty())
+          {
+            element = q_.front();
+            q_.pop();
+          }
+          xSemaphoreGive(m_);
+        }
+        return element;
+      }
+
+    protected:
+      std::queue<T *> q_;
+      SemaphoreHandle_t m_;
+    };
+  } // namespace nimble_tracker
+} // namespace esphome

components/nimble_tracker/string_utils.cpp

@@ -0,0 +1,47 @@
+// Copied from https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/string_utils.cpp
+#include "string_utils.h"
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+
+        static constexpr char hexmap[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'a', 'b', 'c', 'd', 'e', 'f'};
+
+        std::string hexStr(const uint8_t *data, int len)
+        {
+            std::string s(len * 2, ' ');
+            for (int i = 0; i < len; ++i)
+            {
+                s[2 * i] = hexmap[(data[i] & 0xF0) >> 4];
+                s[2 * i + 1] = hexmap[data[i] & 0x0F];
+            }
+            return s;
+        }
+
+        uint8_t hextob(char ch)
+        {
+            if (ch >= '0' && ch <= '9')
+                return ch - '0';
+            if (ch >= 'A' && ch <= 'F')
+                return ch - 'A' + 10;
+            if (ch >= 'a' && ch <= 'f')
+                return ch - 'a' + 10;
+            return 0;
+        }
+
+        bool hextostr(const std::string &hexStr, uint8_t *output, size_t len)
+        {
+            if (len & 1)
+                return false;
+            if (hexStr.length() < len * 2)
+                return false;
+            int k = 0;
+            for (size_t i = 0; i < len * 2; i += 2)
+                output[k++] = (hextob(hexStr[i]) << 4) | hextob(hexStr[i + 1]);
+            return true;
+        }
+
+    } // namespace nimble_tracker
+
+} // namespece esphome

components/nimble_tracker/string_utils.h

@@ -0,0 +1,18 @@
+// Copied from https://github.com/ESPresense/ESPresense/blob/master/lib/BleFingerprint/string_utils.h
+#pragma once
+#include <string>
+
+#define Sprintf(f, ...) ({ char* s; asprintf(&s, f, __VA_ARGS__); std::string r = s; free(s); r; })
+
+namespace esphome
+{
+    namespace nimble_tracker
+    {
+
+        std::string hexStr(const uint8_t *data, int len);
+        uint8_t hextob(char ch);
+        bool hextostr(const std::string &hexStr, uint8_t *output, size_t len);
+
+    } // namespace nimble_tracker
+
+} // namespace esphome