From 37d7a2b40f719c73ff1faa05ed60e0b3f818a0c4 Mon Sep 17 00:00:00 2001 From: Jonas Rabenstein Date: Sun, 21 Dec 2025 02:45:39 +0100 Subject: [PATCH] import fastcon as brmesh --- components/brmesh/__init__.py | 3 + components/brmesh/fastcon_controller.cpp | 275 +++++++++++++++++++++++ components/brmesh/fastcon_controller.h | 90 ++++++++ components/brmesh/fastcon_controller.py | 78 +++++++ components/brmesh/fastcon_light.cpp | 71 ++++++ components/brmesh/fastcon_light.h | 35 +++ components/brmesh/light.py | 37 +++ components/brmesh/protocol.cpp | 60 +++++ components/brmesh/protocol.h | 18 ++ components/brmesh/utils.cpp | 126 +++++++++++ components/brmesh/utils.h | 36 +++ 11 files changed, 829 insertions(+) create mode 100644 components/brmesh/__init__.py create mode 100644 components/brmesh/fastcon_controller.cpp create mode 100644 components/brmesh/fastcon_controller.h create mode 100644 components/brmesh/fastcon_controller.py create mode 100644 components/brmesh/fastcon_light.cpp create mode 100644 components/brmesh/fastcon_light.h create mode 100644 components/brmesh/light.py create mode 100644 components/brmesh/protocol.cpp create mode 100644 components/brmesh/protocol.h create mode 100644 components/brmesh/utils.cpp create mode 100644 components/brmesh/utils.h diff --git a/components/brmesh/__init__.py b/components/brmesh/__init__.py new file mode 100644 index 0000000..9b07c9a --- /dev/null +++ b/components/brmesh/__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_controller.cpp new file mode 100644 index 0000000..4a0180e --- /dev/null +++ b/components/brmesh/fastcon_controller.cpp @@ -0,0 +1,275 @@ +#include "esphome/core/component_iterator.h" +#include "esphome/core/log.h" +#include "esphome/components/light/color_mode.h" +#include "fastcon_controller.h" +#include "protocol.h" + + +namespace esphome +{ + namespace fastcon + { + static const char *const TAG = "fastcon.controller"; + + void FastconController::queueCommand(uint32_t light_id_, const std::vector &data) + { + std::lock_guard lock(queue_mutex_); + if (queue_.size() >= max_queue_size_) + { + ESP_LOGW(TAG, "Command queue full (size=%d), dropping command for light %d", + queue_.size(), light_id_); + return; + } + + Command cmd; + cmd.data = data; + cmd.timestamp = millis(); + cmd.retries = 0; + + queue_.push(cmd); + ESP_LOGV(TAG, "Command queued, queue size: %d", queue_.size()); + } + + void FastconController::clear_queue() + { + std::lock_guard lock(queue_mutex_); + std::queue empty; + std::swap(queue_, empty); + } + + void FastconController::setup() + { + ESP_LOGCONFIG(TAG, "Setting up Fastcon BLE Controller..."); + ESP_LOGCONFIG(TAG, " Advertisement interval: %d-%d", this->adv_interval_min_, this->adv_interval_max_); + ESP_LOGCONFIG(TAG, " Advertisement duration: %dms", this->adv_duration_); + ESP_LOGCONFIG(TAG, " Advertisement gap: %dms", this->adv_gap_); + } + + void FastconController::loop() + { + const uint32_t now = millis(); + + switch (adv_state_) + { + case AdvertiseState::IDLE: + { + std::lock_guard lock(queue_mutex_); + if (queue_.empty()) + return; + + Command cmd = queue_.front(); + queue_.pop(); + + 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, + }; + + uint8_t adv_data_raw[31] = {0}; + 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 + adv_data_raw[adv_data_len++] = cmd.data.size() + 2; + 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 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) + { + 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; + } + + adv_state_ = AdvertiseState::ADVERTISING; + state_start_time_ = now; + ESP_LOGV(TAG, "Started advertising"); + break; + } + + case AdvertiseState::ADVERTISING: + { + if (now - state_start_time_ >= adv_duration_) + { + esp_ble_gap_stop_advertising(); + adv_state_ = AdvertiseState::GAP; + state_start_time_ = now; + ESP_LOGV(TAG, "Stopped advertising, entering gap period"); + } + break; + } + + case AdvertiseState::GAP: + { + if (now - state_start_time_ >= adv_gap_) + { + adv_state_ = AdvertiseState::IDLE; + ESP_LOGV(TAG, "Gap period complete"); + } + break; + } + } + } + + std::vector FastconController::get_light_data(light::LightState *state) + { + std::vector light_data = { + 0, // 0 - On/Off Bit + 7-bit Brightness + 0, // 1 - Blue byte + 0, // 2 - Red byte + 0, // 3 - Green byte + 0, // 4 - Warm byte + 0 // 5 - Cold byte + }; + + // TODO: need to figure out when esphome is changing to white vs setting brightness + + auto values = state->current_values; + + bool is_on = values.is_on(); + if (!is_on) + { + return std::vector({0x00}); + } + + auto color_mode = values.get_color_mode(); + bool has_white = (static_cast(color_mode) & static_cast(light::ColorCapability::WHITE)) != 0; + float brightness = std::min(values.get_brightness() * 127.0f, 127.0f); // clamp the value to at most 127 + light_data[0] = 0x80 + static_cast(brightness); + + if (has_white) + { + return std::vector({static_cast(brightness)}); + // DEBUG: when changing to white mode, this should be the payload: + // ff0000007f7f + } + + bool has_rgb = (static_cast(color_mode) & static_cast(light::ColorCapability::RGB)) != 0; + if (has_rgb) + { + light_data[1] = static_cast(values.get_blue() * 255.0f); + light_data[2] = static_cast(values.get_red() * 255.0f); + light_data[3] = static_cast(values.get_green() * 255.0f); + } + + bool has_cold_warm = (static_cast(color_mode) & static_cast(light::ColorCapability::COLD_WARM_WHITE)) != 0; + if (has_cold_warm) + { + light_data[4] = static_cast(values.get_warm_white() * 255.0f); + light_data[5] = static_cast(values.get_cold_white() * 255.0f); + } + + // TODO figure out if we can use these, and how + bool has_temp = (static_cast(color_mode) & static_cast(light::ColorCapability::COLOR_TEMPERATURE)) != 0; + if (has_temp) + { + float temperature = values.get_color_temperature(); + if (temperature < 153) + { + light_data[4] = 0xff; + light_data[5] = 0x00; + } + else if (temperature > 500) + { + light_data[4] = 0x00; + light_data[5] = 0xff; + } + else + { + // Linear interpolation between (153, 0xff) and (500, 0x00) + light_data[4] = (uint8_t)(((500 - temperature) * 255.0f + (temperature - 153) * 0x00) / (500 - 153)); + light_data[5] = (uint8_t)(((temperature - 153) * 255.0f + (500 - temperature) * 0x00) / (500 - 153)); + } + } + + return light_data; + } + + std::vector FastconController::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 FastconController::generate_command(uint8_t n, uint32_t light_id_, const std::vector &data, bool forward) + { + static uint8_t sequence = 0; + + // 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->mesh_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->mesh_key_[i & 3] ^ body[4 + i]; + } + + // Prepare the final payload with RF protocol formatting + std::vector addr = {DEFAULT_BLE_FASTCON_ADDRESS.begin(), DEFAULT_BLE_FASTCON_ADDRESS.end()}; + return prepare_payload(addr, body); + } + } // namespace fastcon +} // namespace esphome diff --git a/components/brmesh/fastcon_controller.h b/components/brmesh/fastcon_controller.h new file mode 100644 index 0000000..cf78107 --- /dev/null +++ b/components/brmesh/fastcon_controller.h @@ -0,0 +1,90 @@ +#pragma once + +#include +#include +#include +#include "esphome/core/component.h" +#include "esphome/components/esp32_ble/ble.h" + +namespace esphome +{ + namespace fastcon + { + + class FastconController : public Component, public esp32_ble::GAPEventHandler + { + public: + FastconController() = default; + + void setup() override; + void loop() override; + + std::vector get_light_data(light::LightState *state); + std::vector single_control(uint32_t addr, const std::vector &light_data); + + void queueCommand(uint32_t light_id_, const std::vector &data); + + void clear_queue(); + bool is_queue_empty() const + { + std::lock_guard lock(queue_mutex_); + return queue_.empty(); + } + size_t get_queue_size() const + { + std::lock_guard lock(queue_mutex_); + return queue_.size(); + } + void set_max_queue_size(size_t size) { max_queue_size_ = size; } + + void set_mesh_key(std::array key) { mesh_key_ = key; } + void set_adv_interval_min(uint16_t val) { adv_interval_min_ = val; } + void set_adv_interval_max(uint16_t val) + { + adv_interval_max_ = val; + if (adv_interval_max_ < adv_interval_min_) + { + adv_interval_max_ = adv_interval_min_; + } + } + void set_adv_duration(uint16_t val) { adv_duration_ = val; } + void set_adv_gap(uint16_t val) { adv_gap_ = val; } + + protected: + struct Command + { + std::vector data; + uint32_t timestamp; + uint8_t retries{0}; + static constexpr uint8_t MAX_RETRIES = 3; + }; + + std::queue queue_; + mutable std::mutex queue_mutex_; + size_t max_queue_size_{100}; + + 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); + + std::array mesh_key_{}; + + uint16_t adv_interval_min_{0x20}; + uint16_t adv_interval_max_{0x40}; + uint16_t adv_duration_{50}; + uint16_t adv_gap_{10}; + + static const uint16_t MANUFACTURER_DATA_ID = 0xfff0; + }; + + } // namespace fastcon +} // namespace esphome diff --git a/components/brmesh/fastcon_controller.py b/components/brmesh/fastcon_controller.py new file mode 100644 index 0000000..a3361e1 --- /dev/null +++ b/components/brmesh/fastcon_controller.py @@ -0,0 +1,78 @@ +import esphome.codegen as cg +import esphome.config_validation as cv +from esphome.const import CONF_ID +from esphome.core import HexInt + +DEPENDENCIES = ["esp32_ble"] + +CONF_MESH_KEY = "mesh_key" +CONF_ADV_INTERVAL_MIN = "adv_interval_min" +CONF_ADV_INTERVAL_MAX = "adv_interval_max" +CONF_ADV_DURATION = "adv_duration" +CONF_ADV_GAP = "adv_gap" +CONF_MAX_QUEUE_SIZE = "max_queue_size" + +DEFAULT_ADV_INTERVAL_MIN = 0x20 +DEFAULT_ADV_INTERVAL_MAX = 0x40 +DEFAULT_ADV_DURATION = 50 +DEFAULT_ADV_GAP = 10 +DEFAULT_MAX_QUEUE_SIZE = 100 + + +def validate_hex_bytes(value): + if isinstance(value, str): + value = value.replace(" ", "") + if len(value) != 8: + raise cv.Invalid("Mesh key must be exactly 4 bytes (8 hex characters)") + + try: + return HexInt(int(value, 16)) + except ValueError as err: + raise cv.Invalid(f"Invalid hex value: {err}") + raise cv.Invalid("Mesh key must be a string") + + +fastcon_ns = cg.esphome_ns.namespace("fastcon") +FastconController = fastcon_ns.class_("FastconController", cg.Component) + +CONFIG_SCHEMA = cv.Schema( + { + cv.Optional(CONF_ID, default="fastcon_controller"): cv.declare_id( + FastconController + ), + cv.Required(CONF_MESH_KEY): validate_hex_bytes, + cv.Optional( + CONF_ADV_INTERVAL_MIN, default=DEFAULT_ADV_INTERVAL_MIN + ): cv.uint16_t, + cv.Optional( + CONF_ADV_INTERVAL_MAX, default=DEFAULT_ADV_INTERVAL_MAX + ): cv.uint16_t, + cv.Optional(CONF_ADV_DURATION, default=DEFAULT_ADV_DURATION): cv.uint16_t, + cv.Optional(CONF_ADV_GAP, default=DEFAULT_ADV_GAP): cv.uint16_t, + cv.Optional( + CONF_MAX_QUEUE_SIZE, default=DEFAULT_MAX_QUEUE_SIZE + ): cv.positive_int, + } +).extend(cv.COMPONENT_SCHEMA) + + +async def to_code(config): + var = cg.new_Pvariable(config[CONF_ID]) + await cg.register_component(var, config) + + if CONF_MESH_KEY in config: + mesh_key = config[CONF_MESH_KEY] + key_bytes = [(mesh_key >> (i * 8)) & 0xFF for i in range(3, -1, -1)] + cg.add(var.set_mesh_key(key_bytes)) + + if config[CONF_ADV_INTERVAL_MAX] < config[CONF_ADV_INTERVAL_MIN]: + raise cv.Invalid( + f"adv_interval_max ({config[CONF_ADV_INTERVAL_MAX]}) must be >= " + f"adv_interval_min ({config[CONF_ADV_INTERVAL_MIN]})" + ) + + cg.add(var.set_adv_interval_min(config[CONF_ADV_INTERVAL_MIN])) + cg.add(var.set_adv_interval_max(config[CONF_ADV_INTERVAL_MAX])) + cg.add(var.set_adv_duration(config[CONF_ADV_DURATION])) + cg.add(var.set_adv_gap(config[CONF_ADV_GAP])) + cg.add(var.set_max_queue_size(config[CONF_MAX_QUEUE_SIZE])) diff --git a/components/brmesh/fastcon_light.cpp b/components/brmesh/fastcon_light.cpp new file mode 100644 index 0000000..af7a983 --- /dev/null +++ b/components/brmesh/fastcon_light.cpp @@ -0,0 +1,71 @@ +#include +#include "esphome/core/log.h" +#include "fastcon_light.h" +#include "fastcon_controller.h" +#include "utils.h" + +namespace esphome +{ + namespace fastcon + { + static const char *const TAG = "fastcon.light"; + + void FastconLight::setup() + { + if (this->controller_ == nullptr) + { + ESP_LOGE(TAG, "Controller not set for light %d!", this->light_id_); + this->mark_failed(); + return; + } + ESP_LOGCONFIG(TAG, "Setting up Fastcon BLE light (ID: %d)...", this->light_id_); + } + + void FastconLight::set_controller(FastconController *controller) + { + this->controller_ = controller; + } + + light::LightTraits FastconLight::get_traits() + { + auto traits = light::LightTraits(); + traits.set_supported_color_modes({light::ColorMode::RGB, light::ColorMode::WHITE, light::ColorMode::BRIGHTNESS, light::ColorMode::COLD_WARM_WHITE}); + traits.set_min_mireds(153); + traits.set_max_mireds(500); + return traits; + } + + void FastconLight::write_state(light::LightState *state) + { + // Get the light data bits from the state + auto light_data = this->controller_->get_light_data(state); + + // Debug output - print the light state values + bool is_on = (light_data[0] & 0x80) != 0; + float brightness = ((light_data[0] & 0x7F) / 127.0f) * 100.0f; + if (light_data.size() == 1) + { + ESP_LOGD(TAG, "Writing state: light_id=%d, on=%d, brightness=%.1f%%", light_id_, is_on, brightness); + } + else + { + auto r = light_data[2]; + auto g = light_data[3]; + auto b = light_data[1]; + auto warm = light_data[4]; + auto cold = light_data[5]; + ESP_LOGD(TAG, "Writing state: light_id=%d, on=%d, brightness=%.1f%%, rgb=(%d,%d,%d), warm=%d, cold=%d", light_id_, is_on, brightness, r, g, b, warm, cold); + } + + // Generate the advertisement payload + auto adv_data = this->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 + this->controller_->queueCommand(this->light_id_, adv_data); + } + } // namespace fastcon +} // namespace esphome diff --git a/components/brmesh/fastcon_light.h b/components/brmesh/fastcon_light.h new file mode 100644 index 0000000..bf583e2 --- /dev/null +++ b/components/brmesh/fastcon_light.h @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include "esphome/core/component.h" +#include "esphome/components/light/light_output.h" +#include "fastcon_controller.h" + +namespace esphome +{ + namespace fastcon + { + enum class LightState + { + OFF, + WARM_WHITE, + RGB + }; + + class FastconLight : public Component, public light::LightOutput + { + public: + FastconLight(uint8_t light_id) : light_id_(light_id) {} + + void setup() override; + light::LightTraits get_traits() override; + void write_state(light::LightState *state) override; + void set_controller(FastconController *controller); + + protected: + FastconController *controller_{nullptr}; + uint8_t light_id_; + }; + } // namespace fastcon +} // namespace esphome diff --git a/components/brmesh/light.py b/components/brmesh/light.py new file mode 100644 index 0000000..3ff7da4 --- /dev/null +++ b/components/brmesh/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/protocol.cpp new file mode 100644 index 0000000..549b4be --- /dev/null +++ b/components/brmesh/protocol.cpp @@ -0,0 +1,60 @@ +#include +#include "protocol.h" + +namespace esphome +{ + namespace fastcon + { + std::vector get_rf_payload(const std::vector &addr, const std::vector &data) + { + const size_t data_offset = 0x12; + const size_t inverse_offset = 0x0f; + const size_t result_data_size = data_offset + addr.size() + data.size(); + + // Create result buffer including space for checksum + std::vector resultbuf(result_data_size + 2, 0); + + // Set hardcoded values + resultbuf[0x0f] = 0x71; + resultbuf[0x10] = 0x0f; + resultbuf[0x11] = 0x55; + + // Copy address in reverse + for (size_t i = 0; i < addr.size(); i++) + { + resultbuf[data_offset + addr.size() - i - 1] = addr[i]; + } + + // Copy data + std::copy(data.begin(), data.end(), resultbuf.begin() + data_offset + addr.size()); + + // Reverse bytes in specified range + for (size_t i = inverse_offset; i < inverse_offset + addr.size() + 3; i++) + { + resultbuf[i] = reverse_8(resultbuf[i]); + } + + // Add CRC + uint16_t crc = crc16(addr, data); + resultbuf[result_data_size] = crc & 0xFF; + resultbuf[result_data_size + 1] = (crc >> 8) & 0xFF; + + return resultbuf; + } + + std::vector prepare_payload(const std::vector &addr, const std::vector &data) + { + auto payload = get_rf_payload(addr, data); + + // Initialize whitening + WhiteningContext context; + whitening_init(0x25, context); + + // Apply whitening to the payload + whitening_encode(payload, context); + + // Return only the portion after 0xf bytes + return std::vector(payload.begin() + 0xf, payload.end()); + } + } // namespace fastcon +} // namespace esphome \ No newline at end of file diff --git a/components/brmesh/protocol.h b/components/brmesh/protocol.h new file mode 100644 index 0000000..930acab --- /dev/null +++ b/components/brmesh/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/utils.cpp new file mode 100644 index 0000000..371ae09 --- /dev/null +++ b/components/brmesh/utils.cpp @@ -0,0 +1,126 @@ +#include +#include +#include "esphome/core/log.h" +#include "utils.h" + +namespace esphome +{ + namespace fastcon + { + uint8_t reverse_8(uint8_t d) + { + uint8_t result = 0; + for (int i = 0; i < 8; i++) + { + result |= ((d >> i) & 1) << (7 - i); + } + return result; + } + + uint16_t reverse_16(uint16_t d) + { + uint16_t result = 0; + for (int i = 0; i < 16; i++) + { + result |= ((d >> i) & 1) << (15 - i); + } + return result; + } + + uint16_t crc16(const std::vector &addr, const std::vector &data) + { + uint16_t crc = 0xffff; + + // Process address in reverse + for (auto it = addr.rbegin(); it != addr.rend(); ++it) + { + crc ^= (static_cast(*it) << 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; + } + } + } + + // Process data + for (size_t i = 0; i < data.size(); i++) + { + crc ^= (static_cast(reverse_8(data[i])) << 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; + } + } + } + + crc = ~reverse_16(crc); + return crc; + } + + void whitening_init(uint32_t val, WhiteningContext &ctx) + { + uint32_t v0[] = {(val >> 5), (val >> 4), (val >> 3), (val >> 2)}; + + ctx.f_0x0 = 1; + ctx.f_0x4 = v0[0] & 1; + ctx.f_0x8 = v0[1] & 1; + ctx.f_0xc = v0[2] & 1; + ctx.f_0x10 = v0[3] & 1; + ctx.f_0x14 = (val >> 1) & 1; + ctx.f_0x18 = val & 1; + } + + void whitening_encode(std::vector &data, WhiteningContext &ctx) + { + for (size_t i = 0; i < data.size(); i++) + { + uint32_t varC = ctx.f_0xc; + uint32_t var14 = ctx.f_0x14; + uint32_t var18 = ctx.f_0x18; + uint32_t var10 = ctx.f_0x10; + uint32_t var8 = var14 ^ ctx.f_0x8; + uint32_t var4 = var10 ^ ctx.f_0x4; + uint32_t _var = var18 ^ varC; + uint32_t var0 = _var ^ ctx.f_0x0; + + uint8_t c = data[i]; + data[i] = ((c & 0x80) ^ ((var8 ^ var18) << 7)) + ((c & 0x40) ^ (var0 << 6)) + ((c & 0x20) ^ (var4 << 5)) + ((c & 0x10) ^ (var8 << 4)) + ((c & 0x08) ^ (_var << 3)) + ((c & 0x04) ^ (var10 << 2)) + ((c & 0x02) ^ (var14 << 1)) + ((c & 0x01) ^ (var18 << 0)); + + ctx.f_0x8 = var4; + ctx.f_0xc = var8; + ctx.f_0x10 = var8 ^ varC; + ctx.f_0x14 = var0 ^ var10; + ctx.f_0x18 = var4 ^ var14; + ctx.f_0x0 = var8 ^ var18; + ctx.f_0x4 = var0; + } + } + + std::vector vector_to_hex_string(std::vector &data) + { + std::vector hex_str(data.size() * 2 + 1); // Allocate the vector with the required size + for (size_t i = 0; i < data.size(); i++) + { + sprintf(hex_str.data() + (i * 2), "%02X", data[i]); + } + hex_str[data.size() * 2] = '\0'; // Ensure null termination + return hex_str; + } + } // namespace fastcon +} // namespace esphome \ No newline at end of file diff --git a/components/brmesh/utils.h b/components/brmesh/utils.h new file mode 100644 index 0000000..c243d9c --- /dev/null +++ b/components/brmesh/utils.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +namespace esphome +{ + namespace fastcon + { + // Bit manipulation utilities + uint8_t reverse_8(uint8_t d); + uint16_t reverse_16(uint16_t d); + + // CRC calculation + uint16_t crc16(const std::vector &addr, const std::vector &data); + + // Whitening context and functions + struct WhiteningContext + { + uint32_t f_0x0; + uint32_t f_0x4; + uint32_t f_0x8; + uint32_t f_0xc; + uint32_t f_0x10; + uint32_t f_0x14; + uint32_t f_0x18; + + WhiteningContext() : f_0x0(0), f_0x4(0), f_0x8(0), f_0xc(0), f_0x10(0), f_0x14(0), f_0x18(0) {} + }; + + void whitening_init(uint32_t val, WhiteningContext &ctx); + void whitening_encode(std::vector &data, WhiteningContext &ctx); + + std::vector vector_to_hex_string(std::vector &data); + } // namespace fastcon +} // namespace esphome \ No newline at end of file