espilon-source/espilon_bot/components/mod_canbus/canbus_config.c

/*
 * canbus_config.c
 * NVS-backed persistent config for CAN bus module.
 */
#include "sdkconfig.h"

#ifdef CONFIG_MODULE_CANBUS

#include <stdio.h>
#include <string.h>
#include "esp_log.h"
#include "nvs_flash.h"
#include "nvs.h"

#include "canbus_config.h"

#define TAG     "CAN_CFG"
#define NVS_NS  "can_cfg"

/* NVS keys */
#define KEY_BITRATE     "bitrate"
#define KEY_OSC_MHZ     "osc_mhz"
#define KEY_FILTERS     "sw_filters"
#define KEY_FILTER_CNT  "sw_filt_cnt"
#define KEY_ECUS        "ecus"
#define KEY_ECU_CNT     "ecu_cnt"

/* ============================================================
 * Init
 * ============================================================ */

void can_config_init(void)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err == ESP_OK) {
        nvs_close(h);
        ESP_LOGI(TAG, "NVS namespace '%s' ready", NVS_NS);
    } else {
        ESP_LOGW(TAG, "NVS open failed: %s", esp_err_to_name(err));
    }
}

/* ============================================================
 * Bitrate
 * ============================================================ */

int can_config_get_bitrate(void)
{
    nvs_handle_t h;
    int32_t val = CONFIG_CANBUS_DEFAULT_BITRATE;
    if (nvs_open(NVS_NS, NVS_READONLY, &h) == ESP_OK) {
        nvs_get_i32(h, KEY_BITRATE, &val);
        nvs_close(h);
    }
    return (int)val;
}

esp_err_t can_config_set_bitrate(int bitrate)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;
    err = nvs_set_i32(h, KEY_BITRATE, bitrate);
    if (err == ESP_OK) err = nvs_commit(h);
    nvs_close(h);
    return err;
}

/* ============================================================
 * Oscillator
 * ============================================================ */

uint8_t can_config_get_osc_mhz(void)
{
    nvs_handle_t h;
    uint8_t val = CONFIG_CANBUS_OSC_MHZ;
    if (nvs_open(NVS_NS, NVS_READONLY, &h) == ESP_OK) {
        nvs_get_u8(h, KEY_OSC_MHZ, &val);
        nvs_close(h);
    }
    return val;
}

esp_err_t can_config_set_osc_mhz(uint8_t mhz)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;
    err = nvs_set_u8(h, KEY_OSC_MHZ, mhz);
    if (err == ESP_OK) err = nvs_commit(h);
    nvs_close(h);
    return err;
}

/* ============================================================
 * Software Filters (stored as blob of uint32_t array)
 * ============================================================ */

int can_config_get_filters(uint32_t *ids_out, int max_ids)
{
    nvs_handle_t h;
    if (nvs_open(NVS_NS, NVS_READONLY, &h) != ESP_OK) return 0;

    uint8_t cnt = 0;
    nvs_get_u8(h, KEY_FILTER_CNT, &cnt);
    if (cnt == 0 || !ids_out) { nvs_close(h); return 0; }

    if (cnt > max_ids) cnt = max_ids;

    size_t len = cnt * sizeof(uint32_t);
    nvs_get_blob(h, KEY_FILTERS, ids_out, &len);
    nvs_close(h);
    return (int)cnt;
}

static esp_err_t save_filters(nvs_handle_t h, const uint32_t *ids, uint8_t cnt)
{
    esp_err_t err = nvs_set_u8(h, KEY_FILTER_CNT, cnt);
    if (err != ESP_OK) return err;

    if (cnt > 0) {
        err = nvs_set_blob(h, KEY_FILTERS, ids, cnt * sizeof(uint32_t));
    } else {
        nvs_erase_key(h, KEY_FILTERS);
    }
    if (err == ESP_OK) err = nvs_commit(h);
    return err;
}

esp_err_t can_config_add_filter(uint32_t id)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;

    uint32_t ids[CAN_CFG_MAX_SW_FILTERS] = { 0 };
    uint8_t cnt = 0;
    nvs_get_u8(h, KEY_FILTER_CNT, &cnt);
    if (cnt > 0) {
        size_t len = cnt * sizeof(uint32_t);
        nvs_get_blob(h, KEY_FILTERS, ids, &len);
    }

    /* Check duplicate */
    for (int i = 0; i < cnt; i++) {
        if (ids[i] == id) { nvs_close(h); return ESP_OK; }
    }

    if (cnt >= CAN_CFG_MAX_SW_FILTERS) {
        nvs_close(h);
        return ESP_ERR_NO_MEM;
    }

    ids[cnt++] = id;
    err = save_filters(h, ids, cnt);
    nvs_close(h);
    return err;
}

esp_err_t can_config_del_filter(uint32_t id)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;

    uint32_t ids[CAN_CFG_MAX_SW_FILTERS] = { 0 };
    uint8_t cnt = 0;
    nvs_get_u8(h, KEY_FILTER_CNT, &cnt);
    if (cnt > 0) {
        size_t len = cnt * sizeof(uint32_t);
        nvs_get_blob(h, KEY_FILTERS, ids, &len);
    }

    /* Find and remove */
    bool found = false;
    for (int i = 0; i < cnt; i++) {
        if (ids[i] == id) {
            memmove(&ids[i], &ids[i + 1], (cnt - i - 1) * sizeof(uint32_t));
            cnt--;
            found = true;
            break;
        }
    }

    if (found) {
        err = save_filters(h, ids, cnt);
    } else {
        err = ESP_ERR_NOT_FOUND;
    }
    nvs_close(h);
    return err;
}

esp_err_t can_config_clear_filters(void)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;
    err = save_filters(h, NULL, 0);
    nvs_close(h);
    return err;
}

/* ============================================================
 * ECU IDs (same pattern as filters)
 * ============================================================ */

int can_config_get_ecus(uint32_t *ids_out, int max_ids)
{
    nvs_handle_t h;
    if (nvs_open(NVS_NS, NVS_READONLY, &h) != ESP_OK) return 0;

    uint8_t cnt = 0;
    nvs_get_u8(h, KEY_ECU_CNT, &cnt);
    if (cnt == 0 || !ids_out) { nvs_close(h); return 0; }

    if (cnt > max_ids) cnt = max_ids;

    size_t len = cnt * sizeof(uint32_t);
    nvs_get_blob(h, KEY_ECUS, ids_out, &len);
    nvs_close(h);
    return (int)cnt;
}

esp_err_t can_config_add_ecu(uint32_t id)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;

    uint32_t ids[CAN_CFG_MAX_ECUS] = { 0 };
    uint8_t cnt = 0;
    nvs_get_u8(h, KEY_ECU_CNT, &cnt);
    if (cnt > 0) {
        size_t len = cnt * sizeof(uint32_t);
        nvs_get_blob(h, KEY_ECUS, ids, &len);
    }

    for (int i = 0; i < cnt; i++) {
        if (ids[i] == id) { nvs_close(h); return ESP_OK; }
    }

    if (cnt >= CAN_CFG_MAX_ECUS) {
        nvs_close(h);
        return ESP_ERR_NO_MEM;
    }

    ids[cnt++] = id;
    err = nvs_set_u8(h, KEY_ECU_CNT, cnt);
    if (err == ESP_OK) err = nvs_set_blob(h, KEY_ECUS, ids, cnt * sizeof(uint32_t));
    if (err == ESP_OK) err = nvs_commit(h);
    nvs_close(h);
    return err;
}

esp_err_t can_config_clear_ecus(void)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;
    nvs_set_u8(h, KEY_ECU_CNT, 0);
    nvs_erase_key(h, KEY_ECUS);
    err = nvs_commit(h);
    nvs_close(h);
    return err;
}

/* ============================================================
 * Reset All
 * ============================================================ */

esp_err_t can_config_reset_all(void)
{
    nvs_handle_t h;
    esp_err_t err = nvs_open(NVS_NS, NVS_READWRITE, &h);
    if (err != ESP_OK) return err;
    err = nvs_erase_all(h);
    if (err == ESP_OK) err = nvs_commit(h);
    nvs_close(h);
    ESP_LOGI(TAG, "Config reset to defaults");
    return err;
}

/* ============================================================
 * List (for status responses)
 * ============================================================ */

int can_config_list(char *buf, size_t buf_len)
{
    int off = 0;

    off += snprintf(buf + off, buf_len - off,
                    "bitrate=%d\nosc_mhz=%u\n",
                    can_config_get_bitrate(),
                    can_config_get_osc_mhz());

    /* Software filters */
    uint32_t fids[CAN_CFG_MAX_SW_FILTERS];
    int fcnt = can_config_get_filters(fids, CAN_CFG_MAX_SW_FILTERS);
    off += snprintf(buf + off, buf_len - off, "sw_filters=%d:", fcnt);
    for (int i = 0; i < fcnt && off < (int)buf_len - 8; i++) {
        off += snprintf(buf + off, buf_len - off, " 0x%03lX", (unsigned long)fids[i]);
    }
    off += snprintf(buf + off, buf_len - off, "\n");

    /* Discovered ECUs */
    uint32_t eids[CAN_CFG_MAX_ECUS];
    int ecnt = can_config_get_ecus(eids, CAN_CFG_MAX_ECUS);
    off += snprintf(buf + off, buf_len - off, "ecus=%d:", ecnt);
    for (int i = 0; i < ecnt && off < (int)buf_len - 8; i++) {
        off += snprintf(buf + off, buf_len - off, " 0x%03lX", (unsigned long)eids[i]);
    }
    off += snprintf(buf + off, buf_len - off, "\n");

    return off;
}

#endif /* CONFIG_MODULE_CANBUS */