diff --git a/components/brmesh/__init__.py b/components/brmesh/__init__.py index 9b07c9a..eac3d21 100644 --- a/components/brmesh/__init__.py +++ b/components/brmesh/__init__.py @@ -1,3 +1,3 @@ -from .fastcon_controller import CONFIG_SCHEMA, DEPENDENCIES, FastconController, to_code +#from .brmesh import CONFIG_SCHEMA, DEPENDENCIES, to_code -__all__ = ["CONFIG_SCHEMA", "DEPENDENCIES", "FastconController", "to_code"] +#__all__ = ["CONFIG_SCHEMA", "DEPENDENCIES", "to_code"] diff --git a/components/brmesh/brmesh.py b/components/brmesh/brmesh.py new file mode 100644 index 0000000..0376382 --- /dev/null +++ b/components/brmesh/brmesh.py @@ -0,0 +1,67 @@ +import esphome.codegen as cg +import esphome.config_validation as cv +from esphome.const import CONF_ID +from esphome.core import HexInt +from esphome.components import esp32_ble + +if False: + DEPENDENCIES = ["esp32_ble"] + + CONF_KEY = "key" + CONF_ADV = "adv" + CONF_INTERVAL = "interval" + CONF_MIN = "min" + CONF_MAX = "max" + CONF_DURATION = "duration" + CONF_GAP = "gap" + + + def validate_key_string(value): + value = value.replace(" ", "") + if len(value) != 8: + raise cv.Invalid("Key must be exactly 4 bytes (8 hex characters)") + + try: + return [HexInt(int(value[x:x+2], 16)) for x in range(0, len(value), 2)] + except ValueError as err: + raise cv.Invalid(f"Invalid hex value: {err}") + + + def validate_key(value): + if isinstance(value, str): + value = validate_key_string(value) + + if not isinstance(value, list): + raise cv.Invalid("Key must be a list") + + if len(value) != 4: + raise cv.Invalid("Key must have 4 bytes") + + return [ cv.uint8_t(x) for x in value ] + + + brmesh_ns = cg.esphome_ns.namespace("brmesh") + Controller = brmesh_ns.class_("Controller", cg.Component, esp32_ble.GAPEventHandler) + + + CONFIG_SCHEMA = cv.COMPONENT_SCHEMA.extend({ + cv.GenerateID(): cv.declare_id(Controller), + cv.GenerateID(esp32_ble.CONF_BLE_ID): cv.use_id(esp32_ble.ESP32BLE), + cv.Required(CONF_KEY): validate_key, + cv.Optional(CONF_ADV): ADV_SCHEMA, + }) + + + async def to_code(config): + adv = config[CONF_ADV] + interval = adv[CONF_INTERVAL] + + if interval[CONF_MAX] < interval[CONF_MIN]: + raise cv.Invalid( + f"{CONF_ADV}.{CONF_INTERVAL}.{CONF_MIN} ({interval[CONF_MIN]}) must be <= " + f"{CONF_ADV}.{CONF_INTERVLA}.{CONF_MAX} ({interval[CONF_MAX]})" + ) + ble = await cg.get_variable(config[esp32_ble.CONF_BLE_ID]) + + var = cg.new_Pvariable(config[CONF_ID], ble, config[CONF_KEY], interval[CONF_MIN], interval[CONF_MAX], adv[CONF_DURATION].total_milliseconds, adv[CONF_GAP].total_milliseconds) + await cg.register_component(var, config) diff --git a/components/brmesh/controller.cpp b/components/brmesh/controller.cpp new file mode 100644 index 0000000..e62fe98 --- /dev/null +++ b/components/brmesh/controller.cpp @@ -0,0 +1,275 @@ +#if 0 +#include "esphome/core/component_iterator.h" +#include "esphome/core/log.h" +#include "esphome/components/esp32_ble/ble.h" +#include "controller.h" +#include "protocol.h" + +namespace { +const char *const TAG = "brmesh.controller"; + +template +std::enable_if_t<(addr_size == 3 || addr_size == std::dynamic_extent), uint16_t> +crc16(const std::span &addr, const std::span &data, const uint16_t init = 0xFFFF) +{ + constexpr auto step = [](uint16_t result, uint8_t byte)->uint16_t{ + result ^= static_cast(byte) << 8; + for (int j = 0; j < 4; j++) { + uint16_t tmp = result << 1; + if (result & 0x8000) + tmp ^= 0x1021; + result = tmp << 1; + if (tmp & 0x8000) + result ^= 0x1021; + } + return result; + }; + + auto result = init; + + for (const auto &c:addr) + result = step(result, c); + + for (const uint8_t &c:data) + result = step(result, esphome::reverse_bits(c)); + + return ~esphome::reverse_bits(result); +} + +} + + +namespace esphome { +namespace brmesh { +Controller::Controller(esp32_ble::ESP32BLE &ble, + const Key &key, + const uint16_t adv_interval_min, + const uint16_t adv_interval_max, + const uint16_t adv_duration, + const uint16_t adv_gap) + : ble_(ble), key_(key), + adv_interval_min_(adv_interval_min), + adv_interval_max_(adv_interval_max), + adv_duration_(adv_duration), + adv_gap_(adv_duration) +{ +} + +void Controller::setup() +{ + ESP_LOGCONFIG(TAG, "BRmesh Controller:\n" + " key: %02hx:%02hx:%02hx:%02hx\n" + " adv interval: %hd-%hd\n" + " adv duration: %hdms\n" + " adv gap: %hdms", + key_[0], key_[1], key_[2], key_[3], + adv_interval_min_, adv_interval_max_, + adv_duration_, + adv_gap_); + ble_.register_gap_event_handler(this); +} + +void Controller::loop() +{ + auto *light = q_.peek(); + disable_loop(); + if (light == nullptr) + light = q_.peek(); + if (light == nullptr) + return; +} + +void Controller::advertise(Light &light) +{ + esp_ble_adv_params_t adv_params = { + .adv_int_min = adv_interval_min_, + .adv_int_max = adv_interval_max_, + .adv_type = ADV_TYPE_NONCONN_IND, + .own_addr_type = BLE_ADDR_TYPE_PUBLIC, + .peer_addr = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, + .peer_addr_type = BLE_ADDR_TYPE_PUBLIC, + .channel_map = ADV_CHNL_ALL, + .adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY, + }; + + std::array adv_data_raw[31]; + uint8_t adv_data_len = 0; + + // Add flags + adv_data_raw[adv_data_len++] = 2; + adv_data_raw[adv_data_len++] = ESP_BLE_AD_TYPE_FLAG; + adv_data_raw[adv_data_len++] = ESP_BLE_ADV_FLAG_BREDR_NOT_SPT | ESP_BLE_ADV_FLAG_GEN_DISC; + + // Add manufacturer data + const auto cmd_data_len = adv_data_len++; + adv_data_raw[adv_data_len++] = ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE; + adv_data_raw[adv_data_len++] = MANUFACTURER_DATA_ID & 0xFF; + adv_data_raw[adv_data_len++] = (MANUFACTURER_DATA_ID >> 8) & 0xFF; + + const auto cmd = command(std::span(adv_data_raw).subspan(adv_data_len), light); + adv_data_raw[cmd_data_len] = cmd.size_bytes(); + adv_data_len += cmd.size_bytes(); + + esp_err_t err = esp_ble_gap_config_adv_data_raw(adv_data_raw, adv_data_len); + if (err != ESP_OK) + { + ESP_LOGW(TAG, "Error setting raw advertisement data (err=%d): %s", err, esp_err_to_name(err)); + return; + } + + err = esp_ble_gap_start_advertising(&adv_params); + if (err != ESP_OK) + { + ESP_LOGW(TAG, "Error starting advertisement (err=%d): %s", err, esp_err_to_name(err)); + return; + } + + ESP_LOGD(TAG, "advertise id=%hd", light.id()); +} + +void +Controller::gap_event_handler(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param) +{ + switch (event) { + case ESP_GAP_BLE_ADV_START_COMPLETE_EVT: + ESP_LOD(TAG, "advertise: %hdms\n", adv_duration_); + set_timeout(adv_duration_, esp_ble_gap_stop_advertising); + return; + + case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT: + ESP_LOGD(TAG, "advertise gap: %hdms\n", adv_gap_); + set_timeout(adv_gap_, [this](){ + q_.dequeue(); + auto *light = q_.peek(); + if (light == nullptr) + advertise(*light); + }); + return; + + default: + return; + } +} + +void +Controller::enqueue(Light &light) +{ + if (!q_.enqueue(light)) + return; + defer([this](){ advertise(*q_.peek()); }); +} + +std::vector Controller::single_control(uint32_t light_id_, const std::vector &light_data) +{ + std::vector result_data(12); + + result_data[0] = 2 | (((0xfffffff & (light_data.size() + 1)) << 4)); + result_data[1] = light_id_; + std::copy(light_data.begin(), light_data.end(), result_data.begin() + 2); + + // Debug output - print payload as hex + auto hex_str = vector_to_hex_string(result_data).data(); + ESP_LOGD(TAG, "Inner Payload (%d bytes): %s", result_data.size(), hex_str); + + return this->generate_command(5, light_id_, result_data, true); +} + +std::vector Controller::generate_command(uint8_t n, uint32_t light_id_, const std::vector &data, bool forward) +{ + uint8_t& sequence = seq_; + + // Create command body with header + std::vector body(data.size() + 4); + uint8_t i2 = (light_id_ / 256); + + // Construct header + body[0] = (i2 & 0b1111) | ((n & 0b111) << 4) | (forward ? 0x80 : 0); + body[1] = sequence++; // Use and increment sequence number + if (sequence >= 255) + sequence = 1; + + body[2] = this->key_[3]; // Safe key + + // Copy data + std::copy(data.begin(), data.end(), body.begin() + 4); + + // Calculate checksum + uint8_t checksum = 0; + for (size_t i = 0; i < body.size(); i++) + { + if (i != 3) + { + checksum = checksum + body[i]; + } + } + body[3] = checksum; + + // Encrypt header and data + for (size_t i = 0; i < 4; i++) + { + body[i] = DEFAULT_ENCRYPT_KEY[i & 3] ^ body[i]; + } + + for (size_t i = 0; i < data.size(); i++) + { + body[4 + i] = this->key_[i & 3] ^ body[4 + i]; + } + + // Prepare the final payload with RF protocol formatting + std::vector addr = {DEFAULT_BLE_ADDRESS.begin(), DEFAULT_BLE_ADDRESS.end()}; + return prepare_payload(addr, body); +} + +std::span Controller::payload(std::span &&span, const Light &light) +{ + const auto header = span.subspan(0, 3); + header[0] = 0x71; + header[1] = 0x0f; + header[2] = 0x55; + + const auto address = span.subspan(3, 3); + std::reverse_copy(address.begin(), address.end(), + DEFAULT_BLE_ADDRESS.cbegin(), + DEFAULT_BLE_ADDRESS.cend()); + + const auto data = command(span.subspan(6, 10), light); + const auto crc = span.subspan(6 + data.size(), 2); + const auto crc16 = ::crc16(address, data); + crc[0] = static_cast(crc16); + crc[1] = static_cast(crc16 >> 8); + + auto result = span.subspan(0, header.size() + address.size() + data.size() + crc.size()); + + // TODO: adjust seed to skip zero byte warmup + Whitening whitening(0x25); + for (size_t i = 0; i< 16; ++i) + (void) whitening.encode(0x00); + + whitening.encode(result); + + return result; +} + +std::span command(std::span &&span, const Light &light) +{ + static constexpr uint8_t n = 5; // TODO + static constexpr bool forward = true; // TODO + + const auto inner = span.subpsna(4, light.command(span.subspan(4, 6))); + + auto header = span.subspan(0, 4); + header[0] = (i2 & 0b1111) | ((n & 0b111) << 4) | (forward ? 0x80 : 0); + header[1] = seq_++; + header[2] = key_[3]; // safe key + header[3] = crc(inner); + + DEFAULT_ENCRYPT_KEY(header); + key_(inner); + + //prepare(DEFAULT_BLE_ADDRESS, span.subspan(4 + inner.size())); + return span.subspan(0, 4 + inner.size()); +} + +} // namespace brmesh +} // namespace esphome +#endif diff --git a/components/brmesh/controller.h b/components/brmesh/controller.h new file mode 100644 index 0000000..159e7b3 --- /dev/null +++ b/components/brmesh/controller.h @@ -0,0 +1,78 @@ +#pragma once + +#include +#include +#include +#include "esphome/core/component.h" +#include "esphome/components/esp32_ble/ble.h" + +#include "q.h" +#include "key.h" + +namespace esphome { +namespace brmesh { + +class Light; + +class Controller : public Component, public esp32_ble::GAPEventHandler +{ +public: + Controller(esp32_ble::ESP32BLE *ble, const Key &key, uint16_t adv_interval_min, uint16_t adv_interval_max, uint16_t adv_duration, uint16_t adv_gap) + : Controller(*ble, key, adv_interval_min, adv_interval_max, adv_duration, adv_gap) {} + + Controller(esp32_ble::ESP32BLE &ble, const Key &key, uint16_t adv_interval_min, uint16_t adv_interval_max, uint16_t adv_duration, uint16_t adv_gap); + + std::vector single_control(uint32_t addr, const std::vector &light_data); + void enqueue(Light &); + + // interface: Component + void setup() override; + void loop() override; + + // interface: GAPEventHandler + void gap_event_handler(esp_gap_ble_cb_event_t, esp_ble_gap_cb_param_t *); + +protected: + struct Command + { + std::vector data; + uint32_t timestamp; + uint8_t retries{0}; + static constexpr uint8_t MAX_RETRIES = 3; + }; + + enum class AdvertiseState + { + IDLE, + ADVERTISING, + GAP + }; + + AdvertiseState adv_state_{AdvertiseState::IDLE}; + uint32_t state_start_time_{0}; + + // Protocol implementation + std::vector generate_command(uint8_t n, uint32_t light_id_, const std::vector &data, bool forward = true); + + static const uint16_t MANUFACTURER_DATA_ID = 0xfff0; +private: + esp32_ble::ESP32BLE &ble_; + Key key_; + + uint16_t adv_interval_min_; + uint16_t adv_interval_max_; + uint16_t adv_duration_; + uint16_t adv_gap_; + + Q q_ = {}; + uint8_t seq_ = 0; + + void advertise(Light &); + + std::span payload(std::span &span, const Light &light); + + std::span command(std::span &&span, const Light &light); +}; + +} // namespace brmesh +} // namespace esphome diff --git a/components/brmesh/debug.h b/components/brmesh/debug.h new file mode 100644 index 0000000..9e6e4d6 --- /dev/null +++ b/components/brmesh/debug.h @@ -0,0 +1,31 @@ +#pragma once + +#include "esphome/core/log.h" +#include "esphome/core/helpers.h" + +#include + +namespace esphome { +namespace brmesh { + +namespace { +template +void hexdump(const char *TAG, const uint8_t *data, size_t size, const char *fmt, types&&...args) { + // TODO: conditional format_hex_pretty + const auto hex = format_hex_pretty(data, size); + ESP_LOGD(TAG, fmt, std::forward(args)..., hex.c_str()); +} + +template +void hexdump(const char *TAG, const void *data, size_t size, const char *fmt, types&&...args) { + hexdump(TAG, static_cast(data), size, fmt, std::forward(args)...); +} + +template +void hexdump(const char *TAG, const std::span &span, const char *fmt, types&&...args) { + hexdump(TAG, static_cast(&span.front()), span.size_bytes(), fmt, std::forward(args)...); +} + +} // namespace +} // namespace brmesh +} // namespace esphome diff --git a/components/brmesh/fastcon/__init__.py b/components/brmesh/fastcon/__init__.py new file mode 100644 index 0000000..9b07c9a --- /dev/null +++ b/components/brmesh/fastcon/__init__.py @@ -0,0 +1,3 @@ +from .fastcon_controller import CONFIG_SCHEMA, DEPENDENCIES, FastconController, to_code + +__all__ = ["CONFIG_SCHEMA", "DEPENDENCIES", "FastconController", "to_code"] diff --git a/components/brmesh/fastcon_controller.cpp b/components/brmesh/fastcon/fastcon_controller.cpp similarity index 98% rename from components/brmesh/fastcon_controller.cpp rename to components/brmesh/fastcon/fastcon_controller.cpp index 4a0180e..993c079 100644 --- a/components/brmesh/fastcon_controller.cpp +++ b/components/brmesh/fastcon/fastcon_controller.cpp @@ -4,7 +4,6 @@ #include "fastcon_controller.h" #include "protocol.h" - namespace esphome { namespace fastcon @@ -85,14 +84,9 @@ namespace esphome adv_data_raw[adv_data_len++] = MANUFACTURER_DATA_ID & 0xFF; adv_data_raw[adv_data_len++] = (MANUFACTURER_DATA_ID >> 8) & 0xFF; - const auto str = format_hex_pretty(cmd.data); - ESP_LOGI(TAG, "cmd: %s", str.c_str()); - memcpy(&adv_data_raw[adv_data_len], cmd.data.data(), cmd.data.size()); adv_data_len += cmd.data.size(); - const auto adv = format_hex_pretty(adv_data_raw); - ESP_LOGI(TAG, "adv: %s", adv.c_str()); esp_err_t err = esp_ble_gap_config_adv_data_raw(adv_data_raw, adv_data_len); if (err != ESP_OK) { diff --git a/components/brmesh/fastcon_controller.h b/components/brmesh/fastcon/fastcon_controller.h similarity index 95% rename from components/brmesh/fastcon_controller.h rename to components/brmesh/fastcon/fastcon_controller.h index cf78107..cdc7808 100644 --- a/components/brmesh/fastcon_controller.h +++ b/components/brmesh/fastcon/fastcon_controller.h @@ -4,14 +4,14 @@ #include #include #include "esphome/core/component.h" -#include "esphome/components/esp32_ble/ble.h" +#include "esphome/components/esp32_ble_server/ble_server.h" namespace esphome { namespace fastcon { - class FastconController : public Component, public esp32_ble::GAPEventHandler + class FastconController : public Component { public: FastconController() = default; diff --git a/components/brmesh/fastcon_controller.py b/components/brmesh/fastcon/fastcon_controller.py similarity index 100% rename from components/brmesh/fastcon_controller.py rename to components/brmesh/fastcon/fastcon_controller.py diff --git a/components/brmesh/fastcon_light.cpp b/components/brmesh/fastcon/fastcon_light.cpp similarity index 100% rename from components/brmesh/fastcon_light.cpp rename to components/brmesh/fastcon/fastcon_light.cpp diff --git a/components/brmesh/fastcon_light.h b/components/brmesh/fastcon/fastcon_light.h similarity index 100% rename from components/brmesh/fastcon_light.h rename to components/brmesh/fastcon/fastcon_light.h diff --git a/components/brmesh/fastcon/light.py b/components/brmesh/fastcon/light.py new file mode 100644 index 0000000..3ff7da4 --- /dev/null +++ b/components/brmesh/fastcon/light.py @@ -0,0 +1,37 @@ +"""Light platform for Fastcon BLE lights.""" + +import esphome.codegen as cg +import esphome.config_validation as cv +from esphome.components import light +from esphome.const import CONF_LIGHT_ID, CONF_OUTPUT_ID + +from .fastcon_controller import FastconController + +DEPENDENCIES = ["esp32_ble"] +AUTO_LOAD = ["light"] + +CONF_CONTROLLER_ID = "controller_id" + +fastcon_ns = cg.esphome_ns.namespace("fastcon") +FastconLight = fastcon_ns.class_("FastconLight", light.LightOutput, cg.Component) + +CONFIG_SCHEMA = cv.All( + light.BRIGHTNESS_ONLY_LIGHT_SCHEMA.extend( + { + cv.GenerateID(CONF_OUTPUT_ID): cv.declare_id(FastconLight), + cv.Required(CONF_LIGHT_ID): cv.int_range(min=1, max=255), + cv.Optional(CONF_CONTROLLER_ID, default="fastcon_controller"): cv.use_id( + FastconController + ), + } + ).extend(cv.COMPONENT_SCHEMA) +) + + +async def to_code(config): + var = cg.new_Pvariable(config[CONF_OUTPUT_ID], config[CONF_LIGHT_ID]) + await cg.register_component(var, config) + await light.register_light(var, config) + + controller = await cg.get_variable(config[CONF_CONTROLLER_ID]) + cg.add(var.set_controller(controller)) diff --git a/components/brmesh/protocol.cpp b/components/brmesh/fastcon/protocol.cpp similarity index 100% rename from components/brmesh/protocol.cpp rename to components/brmesh/fastcon/protocol.cpp diff --git a/components/brmesh/fastcon/protocol.h b/components/brmesh/fastcon/protocol.h new file mode 100644 index 0000000..930acab --- /dev/null +++ b/components/brmesh/fastcon/protocol.h @@ -0,0 +1,18 @@ +#pragma once + +#include +#include +#include +#include "utils.h" + +namespace esphome +{ + namespace fastcon + { + static const std::array DEFAULT_ENCRYPT_KEY = {0x5e, 0x36, 0x7b, 0xc4}; + static const std::array DEFAULT_BLE_FASTCON_ADDRESS = {0xC1, 0xC2, 0xC3}; + + std::vector get_rf_payload(const std::vector &addr, const std::vector &data); + std::vector prepare_payload(const std::vector &addr, const std::vector &data); + } // namespace fastcon +} // namespace esphome \ No newline at end of file diff --git a/components/brmesh/utils.cpp b/components/brmesh/fastcon/utils.cpp similarity index 100% rename from components/brmesh/utils.cpp rename to components/brmesh/fastcon/utils.cpp diff --git a/components/brmesh/utils.h b/components/brmesh/fastcon/utils.h similarity index 100% rename from components/brmesh/utils.h rename to components/brmesh/fastcon/utils.h diff --git a/components/brmesh/key.h b/components/brmesh/key.h new file mode 100644 index 0000000..77cbac5 --- /dev/null +++ b/components/brmesh/key.h @@ -0,0 +1,28 @@ +#pragma once +#include +#include + +namespace esphome { +namespace brmesh { + +class Key { +public: + constexpr Key(uint8_t a, uint8_t b, uint8_t c, uint8_t d) : data_{a,b,c,d} {} + + template + constexpr std::span operator()(const std::span &span, size_t offset=0) const { + for (size_t i=0; i data_; +}; + +} // namespace brmesh +} // namespace esphome diff --git a/components/brmesh/light.cpp b/components/brmesh/light.cpp new file mode 100644 index 0000000..ad15a1e --- /dev/null +++ b/components/brmesh/light.cpp @@ -0,0 +1,549 @@ +#include "light.h" +#include "whitening.h" +#include "protocol.h" +#include "debug.h" + +#include "esphome/core/log.h" + +namespace { +const char *const TAG = "brmesh.light"; + +} // namespace + +namespace esphome { +namespace brmesh { + +namespace { + +template +uint8_t crc(const std::span &bytes, uint8_t init = 0x00) { + for (const auto& byte:bytes) + init += byte; + return init; +} + +uint16_t crc16(uint8_t byte, uint16_t crc) { + crc ^= static_cast(byte) << 8; + for (int j=0; j<4; ++j) { + uint16_t tmp = crc << 1; + if (crc & 0x8000) + tmp ^= 0x1021; + crc = tmp << 1; + if (tmp & 0x8000) + crc ^= 0x1021; + } + return crc; +} + +uint16_t crc16(const std::array &addr, const std::span &data, const uint16_t seed = 0xFFFF) { + auto crc = seed; + + for (const auto &byte:addr) + crc = crc16(byte, crc); + + for (const auto &byte:data) + crc = crc16(reverse_bits(byte), crc); + + return ~reverse_bits(crc); +} + +template +std::span subspan(const std::span &buffer, size_t offset) { + return std::span(buffer.subspan(offset, size)); +} + +std::span single_light_data(const std::span &buffer, const light::LightState &state) { + const auto &values = state.current_values; + // 0 - On/Off Bit + 7-bit Brightness + // 1 - Blue byte + // 2 - Red byte + // 3 - Green byte + // 4 - Warm byte + // 5 - Cold byte + + if (!values.is_on()) { + const auto data = buffer.subspan(0, 1); + data[0] = 0x00; + return data; + } + + auto color_mode = [mode=values.get_color_mode()](light::ColorMode wanted) { + const auto raw = static_cast(wanted); + return raw == (static_cast(mode) & raw); + }; + float brightness = std::min(values.get_brightness() * 127.0f, 127.0f); // clamp the value to at most 127 + + if (color_mode(light::ColorMode::WHITE)) + { + // TODO: need to figure out when esphome is changing to white vs setting brightness + // when changing to white mode, this should be the payload: + // ff0000007f7f + const auto data = buffer.subspan(0, 1); + data[0] = brightness; + return data; + } + + const auto data = buffer; + data[0] = 0x80 + static_cast(brightness); + if (color_mode(light::ColorMode::RGB)) { + data[1] = static_cast(values.get_blue() * 255.0f); + data[2] = static_cast(values.get_red() * 255.0f); + data[3] = static_cast(values.get_green() * 255.0f); + } else { + data[1] = 0; + data[2] = 0; + data[3] = 0; + } + + if (color_mode(light::ColorMode::COLD_WARM_WHITE)) { + data[4] = static_cast(values.get_warm_white() * 255.0f); + data[5] = static_cast(values.get_cold_white() * 255.0f); + } else if (color_mode(light::ColorMode::COLOR_TEMPERATURE)) { + // TODO figure out if we can use these, and how + float temperature = values.get_color_temperature(); + if (temperature < 153) + { + data[4] = 0xff; + data[5] = 0x00; + } + else if (temperature > 500) + { + data[4] = 0x00; + data[5] = 0xff; + } + else + { + // Linear interpolation between (153, 0xff) and (500, 0x00) + data[4] = (uint8_t)(((500 - temperature) * 255.0f + (temperature - 153) * 0x00) / (500 - 153)); + data[5] = (uint8_t)(((temperature - 153) * 255.0f + (500 - temperature) * 0x00) / (500 - 153)); + } + } else { + data[4] = 0; + data[5] = 0; + } + + return data; + +} + +std::span command(const std::span &buffer, const Key &key, Light::Id id, uint8_t seq, light::LightState &state) { + static constexpr uint8_t n = 5; // TODO + static constexpr bool forward = true; // TODO + const uint8_t i2 = (id.value() / 256); + + if (id.kind() != 'L') + return {}; + + const auto header = buffer.subspan(0, 4); + const auto body = single_light_data(subspan<6>(buffer, 4), state); + + hexdump(TAG, body, "%c%02hhX: body: %s", id.kind(), id.value()); + + header[0] = (i2 & 0b1111) | ((n & 0b111) << 4) | (forward ? 0x80 : 0); + header[1] = seq; + header[2] = key[3]; + header[3] = crc(body); + + const auto result = buffer.subspan(0, 4 + body.size()); + + hexdump(TAG, result, "%c%02hhX: plain: %s", id.kind(), id.value()); + + DEFAULT_ENCRYPT_KEY(header); + key(body); + + hexdump(TAG, result, "%c%02hhX: cipher: %s", id.kind(), id.value()); + + return result; +} + +std::span payload(const std::span &buffer, const Key &key, Light::Id id, uint8_t seq, light::LightState &state) { + // static values + buffer[0] = 0x71; + buffer[1] = 0x0F; + buffer[2] = 0x55; + + const auto address = buffer.subspan(3, 3); + std::reverse_copy(DEFAULT_BLE_ADDRESS.cbegin(), DEFAULT_BLE_ADDRESS.cend(), address.begin()); + + const auto command = brmesh::command(subspan<16>(buffer, 4), key, id, seq, state); + hexdump(TAG, command, "L%hhd: command: %s", id); + + const auto result = buffer.subspan(0, 4 + command.size() + 2); + + uint16_t crc_value = crc16(DEFAULT_BLE_ADDRESS, command); + const auto crc = buffer.subspan(4+command.size(), 2); + crc[0] = crc_value; + crc[1] = crc_value >> 8; + + for (auto &byte:buffer.subspan(0, 6)) + byte = reverse_bits(byte); + + hexdump(TAG, result, "L%hhd: raw: %s", id); + + // TODO: shift seed to drop warmup + Whitening whitening(0x25); + for (size_t i=0; i<0x10; ++i) + whitening.encode(0); + + return whitening.encode(result); +} + +std::span adv_data(const std::span &buffer, const Key &key, Light::Id id, uint8_t seq, light::LightState &state) { + const auto flags = buffer.subspan(0, 3); + const auto length = buffer.subspan(3, 1); + const auto manufacturer = buffer.subspan(4, 3); + const auto payload = brmesh::payload(std::span(buffer.subspan(7)), key, id, seq, state); + + hexdump(TAG, payload, "L%hhd: payload: %s", id); + + flags[0] = 2; + flags[1] = ESP_BLE_AD_TYPE_FLAG; + flags[2] = ESP_BLE_ADV_FLAG_BREDR_NOT_SPT | ESP_BLE_ADV_FLAG_GEN_DISC; + + length[0] = payload.size(); + + manufacturer[0] = ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE; + manufacturer[1] = 0xF0; + manufacturer[2] = 0xFF; + + return buffer.subspan(0, 7 + payload.size()); +} + +} // namespace + +void +Light::adv_event_(esp_ble_gap_cb_param_t::ble_adv_data_raw_cmpl_evt_param &) { + esp_ble_adv_params_t adv_params = { + .adv_int_min = adv_interval_min_, + .adv_int_max = adv_interval_max_, + .adv_type = ADV_TYPE_NONCONN_IND, + .own_addr_type = BLE_ADDR_TYPE_PUBLIC, + .peer_addr = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}, + .peer_addr_type = BLE_ADDR_TYPE_PUBLIC, + .channel_map = ADV_CHNL_ALL, + .adv_filter_policy = ADV_FILTER_ALLOW_SCAN_ANY_CON_ANY, + }; + + const auto err = esp_ble_gap_start_advertising(&adv_params); + if (err != ESP_OK) + ESP_LOGE(TAG, "%c%02hhX: start advertising: %s", id_.kind(), id_.value(), esp_err_to_name(err)); + else + ESP_LOGD(TAG, "%c%02hhX: start advertising %hhd", id_.kind(), id_.value(), count_); +} + +void +Light::adv_event_(esp_ble_gap_cb_param_t::ble_adv_start_cmpl_evt_param ¶m) { + if (param.status != ESP_BT_STATUS_SUCCESS) + // TODO: is param.status a valid esp_err_to_name parameter? + ESP_LOGE(TAG, "adv start: %s", esp_err_to_name(param.status)); + else + ESP_LOGD(TAG, "%c%02hhX: advertise started", id_.kind(), id_.value()); +} + +void +Light::adv_event_(esp_ble_gap_cb_param_t::ble_adv_stop_cmpl_evt_param ¶m) { + if (param.status != ESP_BT_STATUS_SUCCESS) + // TODO: is param.status a valid esp_err_to_name parameter? + ESP_LOGE(TAG, "%c%02hhX: adv stop: %s", id_.kind(), id_.value(), esp_err_to_name(param.status)); + else + ESP_LOGD(TAG, "%c%02hhX: advertise stop", id_.kind(), id_.value()); +} + +// interface: esphome::Component +void +Light::dump_config() { + auto *ble = esphome::esp32_ble::global_ble; + ESP_LOGCONFIG(TAG, + "light %hhd\n" + " key: %02hhx %02hhx %02hhx %02hhx\n" + " seq: %02hhx\n" + " ble: %p", + id_, key_[0], key_[1], key_[2], key_[3], seq_, ble); +} + +void +Light::setup() { + auto *ble = esphome::esp32_ble::global_ble; + + if (!ble) { + ESP_LOGE(TAG, "%c%02hhX: no ble device", id_.kind(), id_.value()); + return; + } + + ble->advertising_register_raw_advertisement_callback([this](bool advertise) { + const bool advertising = advertising_; + ESP_LOGD(TAG, "%c%02hhX: adv callback %s", id_.kind(), id_.value(), advertise ? "on" : "off"); + + if (advertise == advertising) + return; + + advertising_ = advertise; + if (!advertise) + return; + + enable_loop(); + }); +} + +void +Light::loop() { + disable_loop(); + + if (!advertising_) { + ESP_LOGE(TAG, "%c%02hhX: loop while not advertising", id_.kind(), id_.value()); + return; + } + + if (state_ == nullptr) { + ESP_LOGD(TAG, "%c%02hhX: sleep", id_.kind(), id_.value()); + return; + } + + ++count_; + auto *const state = state_; + + const auto seq = seq_++; + std::array buffer; + + auto data = std::visit([buffer=std::span(buffer), this, seq, state](auto &&id) { + return adv_data(buffer, key_, id, seq, *state); + }, id_); + hexdump(TAG, data, "%c%02hhX: advertise: %s", id_.kind(), id_.value()); + + if (data.size() == 0) + return; + + auto err = esp_ble_tx_power_set(ESP_BLE_PWR_TYPE_ADV, tx_power_); + if (err != ESP_OK) + ESP_LOGW(TAG, "%c%02hhX: tx power set: %s", id_.kind(), id_.value(), esp_err_to_name(err)); + + err = esp_ble_gap_config_adv_data_raw(&data.front(), data.size()); + if (err != ESP_OK) + ESP_LOGE(TAG, "%c%02hhX: config adv: %s", id_.kind(), id_.value(), esp_err_to_name(err)); +} + +// interface: esphome::light::LightOutput +light::LightTraits +Light::get_traits() { + auto traits = light::LightTraits(); + traits.set_supported_color_modes({ + esphome::light::ColorMode::RGB, + esphome::light::ColorMode::WHITE, + esphome::light::ColorMode::BRIGHTNESS, + esphome::light::ColorMode::COLD_WARM_WHITE, + }); + traits.set_min_mireds(153); + traits.set_max_mireds(500); + return traits; +} + +void +Light::setup_state(light::LightState *state) { +} + +void +Light::update_state(light::LightState *state) { +} + +#include "./fastcon/fastcon_controller.h" + +void +Light::write_state(light::LightState *state) { + ESP_LOGV(TAG, "%c%02hhX: state %02hx", id_.kind(), id_.value(), state->current_values.get_color_mode()); + + payload_length_ = single_light_data(std::span(payload_), *state).size(); + hexdump(TAG, &payload_.front(), payload_length_, "%c%02hhX: state: %s", id_.kind(), id_.value()); + state_ = state; + count_ = 0; + +#if 0 + switch (state->current_values.get_color_mode()) + // handled in update_state + float red; + float green; + float blue; + float temperatur; + float brightness; + state->current_values_as_rgbct(&red, &green, &blue, &temperatur, &brightness); +#else + static fastcon::FastconController controller = [this]() { + fastcon::FastconController controller; + controller.set_mesh_key(key_); + }(); + + const auto fastcon_data = controller.get_light_data(state); + hexdump(TAG, &fastcon_data.front(), fastcon_data.size(), "%c%02hhX: fastcon: %s", id_.kind(), id_.value()); + + +#endif +} + +// interface: esphome::esp32_ble::GAPEventHandler +void +Light::gap_event_handler(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param) { + if (!advertising_) + return; + + esp_err_t err; + switch (event) { + case ESP_GAP_BLE_ADV_DATA_RAW_SET_COMPLETE_EVT: + adv_event_(param->adv_data_raw_cmpl); + return; + + case ESP_GAP_BLE_ADV_START_COMPLETE_EVT: + adv_event_(param->adv_start_cmpl); + return; + + case ESP_GAP_BLE_ADV_STOP_COMPLETE_EVT: + adv_event_(param->adv_stop_cmpl); + return; + } +} + +} // namespace brmesh +} // namespace esphome + +#if 0 +#include +#include "esphome/core/log.h" +#include "esphome/components/light/light_color_values.h" +#include "light.h" +#include "controller.h" +#include "utils.h" + +namespace { +const char *const TAG = "fastcon.light"; + +const esphome::light::ColorModeMask color_modes[] = { + esphome::light::ColorMode::RGB, + esphome::light::ColorMode::WHITE, + esphome::light::ColorMode::BRIGHTNESS, + esphome::light::ColorMode::COLD_WARM_WHITE, +}; + +} // namespace + +namespace esphome { +namespace brmesh { + +void Light::setup() +{ + ESP_LOGCONFIG(TAG, "id=%hhd", light_id_); +} + +light::LightTraits Light::get_traits() +{ + auto traits = light::LightTraits(); + traits.set_supported_color_modes(color_modes); + traits.set_min_mireds(153); + traits.set_max_mireds(500); + return traits; +} + +void Light::setup_state(light::LightState *state) +{ + state_ = state; +} + +void Light::update_state(light::LightState *state) +{ + state_ = state; + controller_.enqueue(*this); +} + +void Light::write_state(light::LightState *state) +{ + // already queued from update_state +} + +size_t Light::command(std::span &&data) const +{ + if (light_state_ == nullptr) + return 0; + + const auto &values = light_state_->current_values; + // 0 - On/Off Bit + 7-bit Brightness + // 1 - Blue byte + // 2 - Red byte + // 3 - Green byte + // 4 - Warm byte + // 5 - Cold byte + + bool is_on = values.is_on(); + if (!is_on) + { + data[0] = 0x00; + return 1; + } + + auto color_mode = [mode=light::ColorModeMask(values.get_color_mode())](light::ColorCapability cap){ + return light::has_capability(mode, cap); + }; + float brightness = std::min(values.get_brightness() * 127.0f, 127.0f); // clamp the value to at most 127 + + if (color_mode(light::ColorCapability::WHITE)) + { + // TODO: need to figure out when esphome is changing to white vs setting brightness + // when changing to white mode, this should be the payload: + // ff0000007f7f + + data[0] = brightness; + return 1; + } + + data[0] = 0x80 + static_cast(brightness); + if (color_mode(light::ColorCapability::RGB)) { + data[1] = static_cast(values.get_blue() * 255.0f); + data[2] = static_cast(values.get_red() * 255.0f); + data[3] = static_cast(values.get_green() * 255.0f); + } else { + data[1] = 0; + data[2] = 0; + data[3] = 0; + } + + if (color_mode(light::ColorCapability::COLD_WARM_WHITE)) { + data[4] = static_cast(values.get_warm_white() * 255.0f); + data[5] = static_cast(values.get_cold_white() * 255.0f); + } else if (color_mode(light::ColorCapability::COLOR_TEMPERATURE)) { + // TODO figure out if we can use these, and how + float temperature = values.get_color_temperature(); + if (temperature < 153) + { + data[4] = 0xff; + data[5] = 0x00; + } + else if (temperature > 500) + { + data[4] = 0x00; + data[5] = 0xff; + } + else + { + // Linear interpolation between (153, 0xff) and (500, 0x00) + data[4] = (uint8_t)(((500 - temperature) * 255.0f + (temperature - 153) * 0x00) / (500 - 153)); + data[5] = (uint8_t)(((temperature - 153) * 255.0f + (500 - temperature) * 0x00) / (500 - 153)); + } + } else { + data[4] = 0; + data[5] = 0; + } + + return 6; +} +#if 0 + // Generate the advertisement payload + auto adv_data = controller_.single_control(this->light_id_, light_data); + + // Debug output - print payload as hex + auto hex_str = vector_to_hex_string(adv_data).data(); + ESP_LOGD(TAG, "Advertisement Payload (%d bytes): %s", adv_data.size(), hex_str); + + // Send the advertisement + controller_.queueCommand(this->light_id_, adv_data); +} +#endif + +} // namespace brmesh +} // namespace esphome +#endif diff --git a/components/brmesh/light.h b/components/brmesh/light.h new file mode 100644 index 0000000..abed4db --- /dev/null +++ b/components/brmesh/light.h @@ -0,0 +1,88 @@ +#pragma once + +#include +#include +#include +#include +#include "esphome/core/component.h" +#include "esphome/components/light/light_output.h" +#include "esphome/components/esp32_ble/ble.h" + +#include "key.h" + +namespace esphome { +namespace brmesh { + +class Light : public Component, public light::LightOutput, public esp32_ble::GAPEventHandler +{ +public: + enum class Single : uint8_t {}; + enum class Group : uint8_t {}; + class Id : public std::variant { + public: + using variant::variant; + + static constexpr char kind(Single) { return 'L'; } + static constexpr char kind(Group) { return 'G'; } + constexpr char kind() { + return std::visit([](auto &&value) { return Id::kind(value); }, *this); + } + + static constexpr uint8_t value(Single value) { return static_cast(value); } + static constexpr uint8_t value(Group value) { return static_cast(value); } + constexpr uint8_t value() { + return std::visit([](auto &&value) { return Id::value(value); }, *this); + } + }; + + + Light(Id id, const Key &key, + uint16_t adv_interval_min, uint16_t adv_interval_max, + uint16_t adv_duration, uint16_t adv_gap) + : id_(id), key_(key), + adv_interval_min_(adv_interval_min), adv_interval_max_(adv_interval_max), + adv_duration_(adv_duration), adv_gap_(adv_gap) {} + + // interface: Component + void dump_config() override; + float get_setup_priority() const override { + return setup_priority::AFTER_BLUETOOTH; + } + void setup() override; + void loop() override; + + // interface: LightOutput + light::LightTraits get_traits() override; + void setup_state(light::LightState *state) override; + void update_state(light::LightState *state) override; + void write_state(light::LightState *state) override; + + // interface: GAPEventHandler + void gap_event_handler(esp_gap_ble_cb_event_t, esp_ble_gap_cb_param_t *) override; + +private: + light::LightState *state_ = nullptr; + Id id_; + const Key key_; + const uint16_t adv_interval_min_; + const uint16_t adv_interval_max_; + const uint16_t adv_duration_; + const uint16_t adv_gap_; + static constexpr esp_power_level_t tx_power_{}; + uint8_t seq_ = 0; + uint8_t count_ = 0; + bool advertising_ = false; + + std::array payload_ = {}; + size_t payload_length_ = 0; + + void adv_event_(esp_ble_gap_cb_param_t::ble_adv_data_raw_cmpl_evt_param &); + void adv_event_(esp_ble_gap_cb_param_t::ble_adv_start_cmpl_evt_param &); + void adv_event_(esp_ble_gap_cb_param_t::ble_adv_stop_cmpl_evt_param &); + + static std::span adv_data_(const std::span &, uint8_t seq, light::LightState &); + +}; + +} // namespace brmesh +} // namespace esphome diff --git a/components/brmesh/light.py b/components/brmesh/light.py index 3ff7da4..4c7d79f 100644 --- a/components/brmesh/light.py +++ b/components/brmesh/light.py @@ -1,37 +1,92 @@ -"""Light platform for Fastcon BLE lights.""" +"""Light platform for BRmesh BLE lights.""" import esphome.codegen as cg import esphome.config_validation as cv -from esphome.components import light from esphome.const import CONF_LIGHT_ID, CONF_OUTPUT_ID +from esphome.core import HexInt +from esphome.components import light, esp32_ble -from .fastcon_controller import FastconController +#from .brmesh import Controller DEPENDENCIES = ["esp32_ble"] AUTO_LOAD = ["light"] -CONF_CONTROLLER_ID = "controller_id" +CONF_KEY = "key" +CONF_ADV = "adv" +CONF_INTERVAL = "interval" +CONF_MIN = "min" +CONF_MAX = "max" +CONF_DURATION = "duration" +CONF_GAP = "gap" -fastcon_ns = cg.esphome_ns.namespace("fastcon") -FastconLight = fastcon_ns.class_("FastconLight", light.LightOutput, cg.Component) -CONFIG_SCHEMA = cv.All( - light.BRIGHTNESS_ONLY_LIGHT_SCHEMA.extend( - { - cv.GenerateID(CONF_OUTPUT_ID): cv.declare_id(FastconLight), - cv.Required(CONF_LIGHT_ID): cv.int_range(min=1, max=255), - cv.Optional(CONF_CONTROLLER_ID, default="fastcon_controller"): cv.use_id( - FastconController - ), - } - ).extend(cv.COMPONENT_SCHEMA) -) +def validate_key_string(value): + value = value.replace(" ", "") + if len(value) != 8: + raise cv.Invalid("Key must be exactly 4 bytes (8 hex characters)") + + try: + return [HexInt(int(value[x:x+2], 16)) for x in range(0, len(value), 2)] + except ValueError as err: + raise cv.Invalid(f"Invalid hex value: {err}") + + +def validate_key(value): + if isinstance(value, str): + value = validate_key_string(value) + + if not isinstance(value, list): + raise cv.Invalid("Key must be a list") + + if len(value) != 4: + raise cv.Invalid("Key must have 4 bytes") + + return [ cv.uint8_t(x) for x in value ] + + +brmesh_ns = cg.esphome_ns.namespace("brmesh") +Light = brmesh_ns.class_("Light", light.LightOutput, cg.Component) +Single = Light.enum("Single", True) +Group = Light.enum("Group", True); + +ADV_INTERVAL_SCHEMA = cv.Schema({ + cv.Optional(CONF_MIN, default=0x20): cv.uint16_t, + cv.Optional(CONF_MAX, default=0x80): cv.uint16_t, +}) + +ADV_SCHEMA = cv.Schema({ + cv.Optional(CONF_INTERVAL, default=ADV_INTERVAL_SCHEMA({})): ADV_INTERVAL_SCHEMA, + cv.Optional(CONF_DURATION, default="50ms"): cv.positive_time_period, + cv.Optional(CONF_GAP, default="10ms"): cv.positive_time_period, +}) + +CONFIG_SCHEMA = light.light_schema(Light, light.LightType.RGB).extend({ + cv.GenerateID(esp32_ble.CONF_BLE_ID): cv.use_id(esp32_ble.ESP32BLE), + cv.Required(CONF_KEY): validate_key, + cv.Required(CONF_LIGHT_ID): cv.int_range(min=1, max=255), + cv.Optional(CONF_ADV, default=ADV_SCHEMA({})): ADV_SCHEMA, +}) async def to_code(config): - var = cg.new_Pvariable(config[CONF_OUTPUT_ID], config[CONF_LIGHT_ID]) + adv = config[CONF_ADV] + interval = adv[CONF_INTERVAL] + + if interval[CONF_MAX] < interval[CONF_MIN]: + raise cv.Invalid( + f"{CONF_ADV}.{CONF_INTERVAL}.{CONF_MIN} ({interval[CONF_MIN]}) must be <= " + f"{CONF_ADV}.{CONF_INTERVLA}.{CONF_MAX} ({interval[CONF_MAX]})" + ) + ble = await cg.get_variable(config[esp32_ble.CONF_BLE_ID]) + + target = Single(config[CONF_LIGHT_ID]) + var = cg.new_Pvariable(config[CONF_OUTPUT_ID], target, config[CONF_KEY], + interval[CONF_MIN], interval[CONF_MAX], + adv[CONF_DURATION].total_milliseconds, + adv[CONF_GAP].total_milliseconds) + esp32_ble.register_gap_event_handler(ble, var) await cg.register_component(var, config) await light.register_light(var, config) - controller = await cg.get_variable(config[CONF_CONTROLLER_ID]) - cg.add(var.set_controller(controller)) + cg.add_define("USE_ESP32_BLE_ADVERTISING") + cg.add_define("USE_ESP32_BLE_UUID") diff --git a/components/brmesh/protocol.h b/components/brmesh/protocol.h index 930acab..aaee4c0 100644 --- a/components/brmesh/protocol.h +++ b/components/brmesh/protocol.h @@ -3,16 +3,14 @@ #include #include #include -#include "utils.h" +#include "key.h" -namespace esphome -{ - namespace fastcon - { - static const std::array DEFAULT_ENCRYPT_KEY = {0x5e, 0x36, 0x7b, 0xc4}; - static const std::array DEFAULT_BLE_FASTCON_ADDRESS = {0xC1, 0xC2, 0xC3}; +namespace esphome { +namespace brmesh { - std::vector get_rf_payload(const std::vector &addr, const std::vector &data); - std::vector prepare_payload(const std::vector &addr, const std::vector &data); - } // namespace fastcon -} // namespace esphome \ No newline at end of file +static constexpr Key DEFAULT_ENCRYPT_KEY = { 0x5e, 0x36, 0x7b, 0xc4 }; + +static const std::array DEFAULT_BLE_ADDRESS = {0xC1, 0xC2, 0xC3}; + +} // namespace brmesh +} // namespace esphome diff --git a/components/brmesh/q.h b/components/brmesh/q.h new file mode 100644 index 0000000..aedc24f --- /dev/null +++ b/components/brmesh/q.h @@ -0,0 +1,72 @@ +#pragma once + +namespace esphome { +namespace brmesh { + +template +class Q { +public: + class Node; + + constexpr Q() = default; + + bool enqueue(T &); + T *dequeue(); + T *peek(); +private: + Node *head_ = nullptr; + Node **tail_ = &head_; +}; + +template +class Q::Node { +public: + constexpr Node() = default; + + bool enqueue(Node *&head, Node **&tail); + static Node *dequeue(Node *&head, Node **&tail); +private: + Node *next_ = nullptr; +}; + +template +bool Q::enqueue(T &node) { + return static_cast(node).enqueue(head_, tail_); +} + +template +T *Q::dequeue() { + auto *node = Node::dequeue(head_, tail_); + return dynamic_cast(node); +} + +template +T *Q::peek() { + auto *node = head_; + return dynamic_cast(node); +} + + +template +bool Q::Node::enqueue(Node *&head, Node **&tail) { + const bool first = tail == &head; + *tail = this; + tail = &next_; + return first; +} + +template +Q::Node *Q::Node::dequeue(Node *&head, Node **&tail) { + auto *node = head; + if (node == nullptr) + return nullptr; + + head = node->next_; + if (head == nullptr) + tail = &head; + + return node; +} + +} // namespace brmesh +} // namespace esphome diff --git a/components/brmesh/whitening.cpp b/components/brmesh/whitening.cpp new file mode 100644 index 0000000..f174fd4 --- /dev/null +++ b/components/brmesh/whitening.cpp @@ -0,0 +1,31 @@ +#include "whitening.h" +#include "debug.h" + +#include "esphome/core/log.h" +#include "esphome/core/helpers.h" + +namespace { +constexpr auto TAG = "brmesh.whitening"; +} // + +namespace esphome { +namespace brmesh { +Whitening::State +Whitening::use(bool encode, const LUT &lut, State state, std::span bytes) +{ + const bool log = (bytes.size() > 1 || bytes[0] != 0x00); + if (log) + hexdump(TAG, bytes, "%s: input: %s", encode ? "encode" : "decode"); + for (auto &byte:bytes) { + auto &next = lut[state]; + byte = next.lut[byte]; + state = next.state; + } + if (log) + hexdump(TAG, bytes, "%s: output: %s", encode ? "encode" : "decode"); + + return state; +} + +} // brmesh +} // esphome diff --git a/components/brmesh/whitening.h b/components/brmesh/whitening.h new file mode 100644 index 0000000..bf5b71f --- /dev/null +++ b/components/brmesh/whitening.h @@ -0,0 +1,154 @@ +#pragma once +#include +#include +#include + +#include "esphome/core/log.h" + +namespace esphome { +namespace brmesh { + +class Whitening { +public: + using State = uint8_t; + + explicit Whitening(State s = 0) : m_state(s & 0x7F) {} + + // Factory reproduces whitening_init + static Whitening from_val(uint32_t val) { + Context ctx{}; + uint32_t v0[] = { (val >> 5), (val >> 4), (val >> 3), (val >> 2) }; + ctx.f0 = 1; + ctx.f4 = v0[0] & 1; + ctx.f8 = v0[1] & 1; + ctx.fc = v0[2] & 1; + ctx.f10 = v0[3] & 1; + ctx.f14 = (val >> 1) & 1; + ctx.f18 = val & 1; + return Whitening(context_to_index(ctx)); + } + + template + std::span encode(const std::span &input) { + m_state = use(m_state, input); + return input; + } + + uint8_t encode(uint8_t input) { + return encode(std::span(&input, 1))[0]; + } + + template + std::span decode(const std::span &input) { + m_state = use(m_state, input); + return input; + } + + uint8_t decode(uint8_t input) { + return decode(std::span(&input, 1))[0]; + } + +private: + struct Context { uint8_t f0,f4,f8,fc,f10,f14,f18; }; + struct Entry { uint8_t value; State state; }; + static constexpr int STATE_SIZE = 128; + static constexpr int BYTE_SIZE = 256; + struct Next { State state; std::array lut; }; + + using LUT = std::array; + + State m_state; + + static State use(bool encode, const LUT &lut, State state, std::span(bytes)); + + template + static State use(State state, std::span bytes) { + return use(encode, lut, state, std::span(bytes)); + } + +public: + static constexpr State context_to_index(const Context &ctx) { + return (ctx.f0 << 6) | (ctx.f4 << 5) | (ctx.f8 << 4) + | (ctx.fc << 3) | (ctx.f10 << 2) | (ctx.f14 << 1) | ctx.f18; + } + + static constexpr Context index_to_context(State idx) { + return Context{ + uint8_t((idx >> 6) & 1), + uint8_t((idx >> 5) & 1), + uint8_t((idx >> 4) & 1), + uint8_t((idx >> 3) & 1), + uint8_t((idx >> 2) & 1), + uint8_t((idx >> 1) & 1), + uint8_t(idx & 1) + }; + } + + static constexpr uint8_t byte(uint8_t input, const Context &ctx) { + uint32_t varC = ctx.fc; + uint32_t var14 = ctx.f14; + uint32_t var18 = ctx.f18; + uint32_t var10 = ctx.f10; + uint32_t var8 = var14 ^ ctx.f8; + uint32_t var4 = var10 ^ ctx.f4; + uint32_t _var = var18 ^ varC; + uint32_t var0 = _var ^ ctx.f0; + + return ((input & 0x80) ^ ((var8 ^ var18) << 7)) + | ((input & 0x40) ^ (var0 << 6)) + | ((input & 0x20) ^ (var4 << 5)) + | ((input & 0x10) ^ (var8 << 4)) + | ((input & 0x08) ^ (_var << 3)) + | ((input & 0x04) ^ (var10 << 2)) + | ((input & 0x02) ^ (var14 << 1)) + | ((input & 0x01) ^ var18); + } + + static constexpr Context next(const Context &ctx) { + uint32_t varC = ctx.fc; + uint32_t var14 = ctx.f14; + uint32_t var18 = ctx.f18; + uint32_t var10 = ctx.f10; + uint32_t var8 = var14 ^ ctx.f8; + uint32_t var4 = var10 ^ ctx.f4; + uint32_t _var = var18 ^ varC; + uint32_t var0 = _var ^ ctx.f0; + + Context next{}; + next.f8 = var4; + next.fc = var8; + next.f10 = var8 ^ varC; + next.f14 = var0 ^ var10; + next.f18 = var4 ^ var14; + next.f0 = var8 ^ var18; + next.f4 = var0; + return next; + } + +private: + static constexpr LUT create_lut(bool forward) { + LUT lut{}; + for (uint16_t s = 0; s < lut.size(); ++s) { + auto ctx = Whitening::index_to_context(uint8_t(s)); + lut[s].state = Whitening::context_to_index(Whitening::next(ctx)); + + for (uint16_t b = 0; b < lut[s].lut.size(); ++b) { + uint8_t enc = Whitening::byte(uint8_t(b), ctx); + + const auto key = forward ? uint8_t(b) : enc; + const auto value = forward ? enc : uint8_t(b); + lut[s].lut[key] = value; + } + } + return lut; + } + + template + static const LUT lut; +}; + +template +const Whitening::LUT Whitening::lut = Whitening::create_lut(forward); + +} // namespace: brmesh +} // namespace: esphome