utun2/lib/ble/ble_transport.c

#include "ble_backend.h"
#include "../u_async.h"
#include "../mem.h"
#include "../debug_config.h"
#include <string.h>
#define CAT DEBUG_CATEGORY_DISCOVERY
#define HELLO 1
#define ACCEPT 2
#define FRAME 3
#define HEADER 7
#define DEADLINE_US 15000000ULL
struct ble_connection {
    ble_link_id id;
    uint64_t peer, deadline, rx_deadline;
    struct ble_endpoint endpoint;
    struct ll_queue *tx, *rx;
    struct ll_entry* current;
    uint16_t sequence, offset, pending_bytes, rx_sequence, rx_size, rx_offset, dropped_sequence;
    unsigned int capacity;
    int outgoing, opened, ready, pending, blocked, control_pending, dropping;
    uint8_t control[17], assembly[BLE_FRAME_MAX];
};
struct ble_transport {
    struct UASYNC* ua;
    struct ble_config config;
    char adapter[64];
    struct ble_backend* backend;
    struct ble_connection links[BLE_LINKS_MAX];
    ble_link_id sequence;
    void* timer;
    int started, ready, reject_incoming;
};
static uint16_t read16(const uint8_t* p) { return (uint16_t)((p[0] << 8) | p[1]); }
static void write16(uint8_t* p, uint16_t v) { p[0] = v >> 8; p[1] = v; }
static uint64_t read64(const uint8_t* p) {
    uint64_t v = 0;
    for (int i = 0; i < 8; i++) v = (v << 8) | p[i];
    return v;
}
static void write64(uint8_t* p, uint64_t v) { for (int i = 7; i >= 0; i--) { p[i] = v; v >>= 8; } }
static void release_entry(struct ll_entry* e) { if (e) { queue_dgram_free(e); queue_entry_free(e); } }
static struct ble_connection* lookup(struct ble_transport* t, ble_link_id id) {
    if (t && id) for (int i = 0; i < BLE_LINKS_MAX; i++) if (t->links[i].id == id) return &t->links[i];
    return NULL;
}
static void close_link(struct ble_transport* t, struct ble_connection* c, const char* reason) {
    ble_link_id id = c->id;
    uint64_t peer = c->peer;
    DEBUG_INFO(CAT, "BLE closed link=%llu peer=%016llx reason=%s", (unsigned long long)id, (unsigned long long)peer, reason);
    if (t->backend) ble_backend_close(t->backend, id);
    release_entry(c->current);
    queue_clear(c->tx); queue_free(c->tx);
    queue_clear(c->rx); queue_free(c->rx);
    memset(c, 0, sizeof(*c));
    if (t->config.callbacks.connection) t->config.callbacks.connection(t->config.arg, id, peer, BLE_CLOSED, reason);
}
static struct ble_connection* new_link(struct ble_transport* t, const struct ble_endpoint* ep, int outgoing, uint64_t peer) {
    if (!t || !t->started || !ep || !memchr(ep->id, 0, sizeof(ep->id)) || !ep->id[0]) {
        DEBUG_ERROR(CAT, "BLE invalid connection request"); return NULL;
    }
    for (int i = 0; i < BLE_LINKS_MAX; i++) if (t->links[i].id && !strcmp(t->links[i].endpoint.id, ep->id)) {
        DEBUG_WARN(CAT, "BLE duplicate connection endpoint=%s", ep->id); return NULL;
    }
    for (int i = 0; i < BLE_LINKS_MAX; i++) if (!t->links[i].id) {
        struct ble_connection* c = &t->links[i];
        c->tx = queue_new(t->ua, 0, 0, 0, "ble_tx"); c->rx = queue_new(t->ua, 0, 0, 0, "ble_rx");
        if (!c->tx || !c->rx) {
            DEBUG_ERROR(CAT, "BLE queue allocation failed"); queue_free(c->tx); queue_free(c->rx);
            memset(c, 0, sizeof(*c)); return NULL;
        }
        c->id = ++t->sequence; c->peer = peer; c->endpoint = *ep; c->outgoing = outgoing;
        c->capacity = 20; c->deadline = get_time_us() + DEADLINE_US;
        DEBUG_INFO(CAT, "BLE connecting link=%llu endpoint=%s outgoing=%d", (unsigned long long)c->id, ep->id, outgoing);
        if (t->config.callbacks.connection) t->config.callbacks.connection(t->config.arg, c->id, peer, BLE_STARTING, "connecting");
        return c;
    }
    DEBUG_WARN(CAT, "BLE connection limit=%d", BLE_LINKS_MAX); return NULL;
}
static void pump(struct ble_transport* t, struct ble_connection* c) {
    if (!c->opened || c->pending) return;
    uint8_t value[BLE_VALUE_MAX];
    size_t len;
    c->pending_bytes = 0;
    if (c->control_pending) { memcpy(value, c->control, 17); len = 17; }
    else {
        if (!c->ready) return;
        if (!c->current) {
            c->current = queue_data_get(c->tx); c->offset = 0;
            if (!c->current) return;
            ++c->sequence;
        }
        size_t n = c->current->len - c->offset;
        if (n > c->capacity - HEADER) n = c->capacity - HEADER;
        value[0] = FRAME; write16(value + 1, c->sequence); write16(value + 3, c->current->len); write16(value + 5, c->offset);
        memcpy(value + HEADER, c->current->dgram + c->offset, n); len = HEADER + n; c->pending_bytes = n;
    }
    int result = ble_backend_send(t->backend, c->id, value, len);
    if (result == BLE_OK) { c->pending = 1; c->deadline = get_time_us() + DEADLINE_US; }
    else { DEBUG_ERROR(CAT, "BLE backend send failed link=%llu result=%d", (unsigned long long)c->id, result);
        close_link(t, c, "backend send failed"); }
}
static void tick(void* arg) {
    struct ble_transport* t = arg; t->timer = NULL;
    uint64_t now = get_time_us();
    for (int i = 0; i < BLE_LINKS_MAX; i++) {
        struct ble_connection* c = &t->links[i];
        if (!c->id) continue;
        if ((!c->ready || c->pending) && now >= c->deadline) {
            DEBUG_WARN(CAT, "BLE handshake/send timeout link=%llu pending=%d", (unsigned long long)c->id, c->pending);
            close_link(t, c, "operation timeout"); continue;
        }
        if (c->rx_size && now >= c->rx_deadline) {
            DEBUG_WARN(CAT, "BLE frame timeout link=%llu received=%u expected=%u",
                       (unsigned long long)c->id, c->rx_offset, c->rx_size);
            c->dropping = 1; c->dropped_sequence = c->rx_sequence;
            c->rx_size = c->rx_offset = 0;
        }
    }
    t->timer = uasync_set_timeout(t->ua, 1000, t, tick, "ble_tick");
    if (!t->timer) { DEBUG_ERROR(CAT, "BLE timer allocation failed"); ble_backend_status(t, 0, "timer allocation failed"); }
}
struct ble_transport* ble_create(struct UASYNC* ua, const struct ble_config* config) {
    if (!ua || !config || !config->node_id || (config->adapter && strlen(config->adapter) >= 64)) {
        DEBUG_ERROR(CAT, "BLE create invalid configuration"); return NULL;
    }
    struct ble_transport* t = u_calloc(1, sizeof(*t));
    if (!t) { DEBUG_ERROR(CAT, "BLE create allocation failed"); return NULL; }
    t->ua = ua; t->config = *config;
    if (config->adapter) { strcpy(t->adapter, config->adapter); t->config.adapter = t->adapter; }
    return t;
}
int ble_start(struct ble_transport* t) {
    if (!t || t->started) { DEBUG_ERROR(CAT, "BLE invalid/repeated start"); return BLE_ERROR; }
    t->started = 1;
    t->backend = ble_backend_create(t, t->ua, &t->config);
    if (!t->backend) { t->started = 0; DEBUG_ERROR(CAT, "BLE backend creation failed"); return BLE_ERROR; }
    t->timer = uasync_set_timeout(t->ua, 1000, t, tick, "ble_tick");
    if (!t->timer) { DEBUG_ERROR(CAT, "BLE timer allocation failed"); ble_stop(t); return BLE_ERROR; }
    if (t->config.callbacks.status) t->config.callbacks.status(t->config.arg, BLE_STARTING, "registering BLE service");
    return BLE_OK;
}
void ble_stop(struct ble_transport* t) {
    if (!t || !t->started) return;
    t->started = t->ready = 0;
    if (t->timer) { uasync_cancel_timeout(t->ua, t->timer); t->timer = NULL; }
    for (int i = 0; i < BLE_LINKS_MAX; i++) if (t->links[i].id) close_link(t, &t->links[i], "transport stopped");
    ble_backend_destroy(t->backend); t->backend = NULL;
    if (t->config.callbacks.status) t->config.callbacks.status(t->config.arg, BLE_CLOSED, "stopped");
}
void ble_set_incoming(struct ble_transport* t, int allowed) {
    if (!t) { DEBUG_ERROR(CAT, "BLE incoming policy: null transport"); return; }
    t->reject_incoming = !allowed;
    DEBUG_INFO(CAT, "BLE incoming allowed=%d", !!allowed);
    if (!allowed) for (int i = 0; i < BLE_LINKS_MAX; i++)
        if (t->links[i].id && !t->links[i].outgoing) close_link(t, &t->links[i], "incoming disabled");
}
void ble_destroy(struct ble_transport* t) { if (t) { ble_stop(t); u_free(t); } }
int ble_advertise(struct ble_transport* t, const void* data, size_t len) {
    if (!t || !t->ready) { DEBUG_WARN(CAT, "BLE advertise before ready"); return BLE_NOT_READY; }
    if (len > BLE_BROADCAST_MAX || (len && !data)) { DEBUG_ERROR(CAT, "BLE invalid advertisement length=%zu", len); return BLE_ERROR; }
    return ble_backend_advertise(t->backend, data, len);
}
ble_link_id ble_connect(struct ble_transport* t, const struct ble_endpoint* ep, uint64_t peer) {
    if (!t || !t->ready) { DEBUG_WARN(CAT, "BLE connect before ready"); return 0; }
    struct ble_connection* c = new_link(t, ep, 1, peer);
    if (!c) return 0;
    ble_link_id id = c->id;
    if (ble_backend_connect(t->backend, id, ep) != BLE_OK) { close_link(t, c, "connect rejected"); return 0; }
    return id;
}
void ble_disconnect(struct ble_transport* t, ble_link_id id) {
    struct ble_connection* c = lookup(t, id);
    if (c) close_link(t, c, "API disconnect");
    else DEBUG_WARN(CAT, "BLE disconnect unknown link=%llu", (unsigned long long)id);
}
int ble_tx_put(struct ble_transport* t, ble_link_id id, struct ll_entry* e) {
    struct ble_connection* c = lookup(t, id);
    if (!c || !c->ready) { DEBUG_WARN(CAT, "BLE TX link not ready=%llu", (unsigned long long)id); return BLE_NOT_READY; }
    if (!e || e->owner || e->next || e->prev || !e->dgram || !e->len || e->len > BLE_FRAME_MAX || e->len > e->memlen) {
        DEBUG_ERROR(CAT, "BLE invalid TX frame link=%llu", (unsigned long long)id); return BLE_ERROR;
    }
    if (queue_entry_count(c->tx) + !!c->current >= BLE_QUEUE_MAX) {
        if (!c->blocked) DEBUG_DEBUG(CAT, "BLE TX paused link=%llu packets=%d", (unsigned long long)id, BLE_QUEUE_MAX);
        c->blocked = 1; return BLE_WOULD_BLOCK;
    }
    /* Без лимита/индекса/consumer put не имеет отказа после проверок и не вызывает чужой callback. */
    if (queue_data_put(c->tx, e) != 0) { DEBUG_ERROR(CAT, "BLE TX queue invariant failed"); return BLE_ERROR; }
    pump(t, c); return BLE_OK;
}
struct ll_entry* ble_rx_get(struct ble_transport* t, ble_link_id id) {
    struct ble_connection* c = lookup(t, id);
    return c ? queue_data_get(c->rx) : NULL;
}
ble_link_id ble_backend_incoming(struct ble_transport* t, const struct ble_endpoint* ep) {
    if (t->reject_incoming) { DEBUG_INFO(CAT, "BLE incoming rejected by policy"); return 0; }
    struct ble_connection* c = new_link(t, ep, 0, 0); return c ? c->id : 0;
}
void ble_backend_status(struct ble_transport* t, int ready, const char* reason) {
    t->ready = ready;
    if (!ready) {
        DEBUG_ERROR(CAT, "BLE radio failed: %s", reason);
        for (int i = 0; i < BLE_LINKS_MAX; i++) if (t->links[i].id) close_link(t, &t->links[i], reason);
    } else DEBUG_INFO(CAT, "BLE radio ready node=%016llx", (unsigned long long)t->config.node_id);
    if (t->config.callbacks.status) t->config.callbacks.status(t->config.arg, ready ? BLE_READY : BLE_CLOSED, reason);
}
void ble_backend_broadcast(struct ble_transport* t, const struct ble_endpoint* ep, const uint8_t* data, size_t len, int rssi) {
    if (len > BLE_BROADCAST_MAX) { DEBUG_WARN(CAT, "BLE oversized broadcast bytes=%zu", len); return; }
    if (t->config.callbacks.broadcast) t->config.callbacks.broadcast(t->config.arg, ep, data, len, rssi);
}
void ble_backend_advertising(struct ble_transport* t, enum ble_state state, const char* reason) {
    DEBUG_INFO(CAT, "BLE advertising state=%d reason=%s", state, reason);
    if (t->config.callbacks.advertising) t->config.callbacks.advertising(t->config.arg, state, reason);
}
void ble_backend_opened(struct ble_transport* t, ble_link_id id, unsigned int mtu) {
    struct ble_connection* c = lookup(t, id);
    if (!c) return;
    if (mtu < 23) { DEBUG_ERROR(CAT, "BLE invalid MTU=%u", mtu); close_link(t, c, "invalid MTU"); return; }
    c->capacity = mtu - 3; if (c->capacity > BLE_VALUE_MAX) c->capacity = BLE_VALUE_MAX;
    c->opened = 1;
    DEBUG_DEBUG(CAT, "BLE GATT open link=%llu mtu=%u outgoing=%d", (unsigned long long)id, mtu, c->outgoing);
    if (c->outgoing) {
        c->control[0] = HELLO; write64(c->control + 1, t->config.node_id); write64(c->control + 9, c->peer);
        c->control_pending = 1;
    }
    pump(t, c);
}
void ble_backend_received(struct ble_transport* t, ble_link_id id, const uint8_t* data, size_t len) {
    struct ble_connection* c = lookup(t, id);
    if (!c) return;
    if (len == 17 && !c->ready && ((data[0] == HELLO && !c->outgoing) || (data[0] == ACCEPT && c->outgoing))) {
        uint64_t peer = read64(data + 1), target = read64(data + 9);
        if (!peer || peer == t->config.node_id || (target && target != t->config.node_id) || (c->peer && peer != c->peer)) {
            DEBUG_WARN(CAT, "BLE invalid HELLO identity link=%llu", (unsigned long long)id); close_link(t, c, "invalid identity"); return;
        }
        if (t->config.callbacks.validate && !t->config.callbacks.validate(t->config.arg, id, peer)) {
            DEBUG_WARN(CAT, "BLE validation rejected link=%llu", (unsigned long long)id); close_link(t, c, "validation rejected"); return;
        }
        c->peer = peer; c->ready = 1;
        if (!c->outgoing) {
            c->control[0] = ACCEPT; write64(c->control + 1, t->config.node_id); write64(c->control + 9, peer); c->control_pending = 1;
        }
        DEBUG_INFO(CAT, "BLE session ready link=%llu peer=%016llx", (unsigned long long)id, (unsigned long long)peer);
        if (t->config.callbacks.connection) t->config.callbacks.connection(t->config.arg, id, peer, BLE_READY, "HELLO accepted");
        pump(t, c); return;
    }
    if (!c->ready || len <= HEADER || len > c->capacity || data[0] != FRAME) {
        DEBUG_WARN(CAT, "BLE invalid value link=%llu bytes=%zu ready=%d", (unsigned long long)id, len, c->ready);
        c->rx_size = c->rx_offset = 0; return;
    }
    uint16_t seq = read16(data + 1), total = read16(data + 3), offset = read16(data + 5);
    size_t n = len - HEADER;
    if (offset && c->dropping && c->dropped_sequence == seq) {
        DEBUG_TRACE(CAT, "BLE discarded frame tail link=%llu seq=%u offset=%u", (unsigned long long)id, seq, offset); return;
    }
    if (!offset) c->dropping = 0;
    if (!total || total > BLE_FRAME_MAX || offset > total || n > (size_t)(total - offset)) goto invalid;
    if (!offset) {
        if (c->rx_size) DEBUG_WARN(CAT, "BLE incomplete frame replaced link=%llu", (unsigned long long)id);
        c->rx_sequence = seq; c->rx_size = total; c->rx_offset = 0; c->rx_deadline = get_time_us() + DEADLINE_US;
    }
    if (c->rx_size != total || c->rx_sequence != seq || c->rx_offset != offset) goto invalid;
    memcpy(c->assembly + offset, data + HEADER, n); c->rx_offset += n;
    if (c->rx_offset != total) return;
    c->rx_size = c->rx_offset = 0;
    if (queue_entry_count(c->rx) >= BLE_QUEUE_MAX) { DEBUG_WARN(CAT, "BLE RX full: frame dropped link=%llu", (unsigned long long)id); return; }
    struct ll_entry* entry = ll_alloc_lldgram(total);
    if (!entry) { DEBUG_ERROR(CAT, "BLE RX allocation failed bytes=%u", total); return; }
    memcpy(entry->dgram, c->assembly, total); entry->len = total;
    if (queue_data_put(c->rx, entry) != 0) { DEBUG_ERROR(CAT, "BLE RX queue invariant failed"); return; }
    DEBUG_DEBUG(CAT, "BLE RX frame link=%llu seq=%u bytes=%u queued=%d", (unsigned long long)id, seq, total, queue_entry_count(c->rx));
    if (t->config.callbacks.received) t->config.callbacks.received(t->config.arg, id);
    return;
invalid:
    DEBUG_WARN(CAT, "BLE fragment dropped link=%llu seq=%u offset=%u expected=%u total=%u bytes=%zu",
               (unsigned long long)id, seq, offset, c->rx_offset, total, n);
    c->rx_size = c->rx_offset = 0;
    c->dropping = 1; c->dropped_sequence = seq;
}
void ble_backend_sent(struct ble_transport* t, ble_link_id id, int success) {
    struct ble_connection* c = lookup(t, id);
    if (!c) return;
    if (!success || !c->pending) { DEBUG_WARN(CAT, "BLE send completion failed link=%llu", (unsigned long long)id);
        close_link(t, c, "send failed"); return; }
    c->pending = 0;
    if (!c->pending_bytes) c->control_pending = 0;
    else if (c->current) {
        c->offset += c->pending_bytes;
        if (c->offset == c->current->len) {
            DEBUG_DEBUG(CAT, "BLE TX frame link=%llu seq=%u bytes=%u", (unsigned long long)id, c->sequence, c->current->len);
            release_entry(c->current); c->current = NULL;
        }
    }
    if (c->blocked && queue_entry_count(c->tx) + !!c->current < BLE_QUEUE_MAX / 2) {
        c->blocked = 0; DEBUG_DEBUG(CAT, "BLE TX resumed link=%llu", (unsigned long long)id);
        if (t->config.callbacks.writable) t->config.callbacks.writable(t->config.arg, id);
    }
    pump(t, c);
}
void ble_backend_closed(struct ble_transport* t, ble_link_id id, const char* reason) {
    struct ble_connection* c = lookup(t, id); if (c) close_link(t, c, reason);
}