templates/c/set-protocol/src/pcan_gas.c

#include "pcan_gas.h"
#include "pcan_id.h"

bool pcan_gas_map_validate(const pcan_gas_map_t *map)
{
    if ((map == NULL) || ((map->count != 0U) && (map->regions == NULL))) {
        return false;
    }
    if (map->count > PCAN_GAS_MAX_REGIONS) {
        return false;
    }

    uint32_t prev_end = 0U;
    for (uint16_t i = 0U; i < map->count; i++) {
        const pcan_gas_region_t *r = &map->regions[i];
        if (r->count == 0U) {
            return false;
        }
        /* Регион не должен уходить за 0xFFFF. */
        if (((uint32_t)r->base + r->count) > 0x10000UL) {
            return false;
        }
        if ((r->storage == NULL) && (r->read == NULL) && (r->write == NULL)) {
            return false;
        }
        if ((i != 0U) && ((uint32_t)r->base < prev_end)) {
            return false;   /* не отсортировано либо перекрытие */
        }
        prev_end = (uint32_t)r->base + r->count;
    }
    return true;
}

const pcan_gas_region_t *pcan_gas_find(const pcan_gas_map_t *map, uint16_t addr)
{
    if ((map == NULL) || (map->regions == NULL)) {
        return NULL;
    }
    /* Регионы отсортированы, поэтому двоичный поиск. */
    uint16_t lo = 0U;
    uint16_t hi = map->count;
    while (lo < hi) {
        uint16_t mid = (uint16_t)(lo + ((hi - lo) / 2U));
        const pcan_gas_region_t *r = &map->regions[mid];
        if (addr < r->base) {
            hi = mid;
        } else if ((uint32_t)addr >= ((uint32_t)r->base + r->count)) {
            lo = (uint16_t)(mid + 1U);
        } else {
            return r;
        }
    }
    return NULL;
}

pcan_gas_status_t pcan_gas_read(const pcan_gas_map_t *map, uint16_t addr,
                                uint16_t *value)
{
    const pcan_gas_region_t *r = pcan_gas_find(map, addr);
    if (r == NULL) {
        return PCAN_GAS_NO_REG;
    }
    if ((r->flags & PCAN_GAS_WRONLY) != 0U) {
        return PCAN_GAS_WRITE_ONLY;
    }

    uint16_t offset = (uint16_t)(addr - r->base);
    if (r->read != NULL) {
        return r->read(r, offset, value);
    }
    if (r->storage != NULL) {
        *value = r->storage[offset];
        return PCAN_GAS_OK;
    }
    return PCAN_GAS_WRITE_ONLY;
}

pcan_gas_status_t pcan_gas_write(const pcan_gas_map_t *map, uint16_t addr,
                                 uint16_t value)
{
    const pcan_gas_region_t *r = pcan_gas_find(map, addr);
    if (r == NULL) {
        return PCAN_GAS_NO_REG;
    }
    if ((r->flags & PCAN_GAS_RDONLY) != 0U) {
        return PCAN_GAS_READ_ONLY;
    }

    uint16_t offset = (uint16_t)(addr - r->base);
    if (r->write != NULL) {
        return r->write(r, offset, value);
    }
    if (r->storage != NULL) {
        r->storage[offset] = value;
        return PCAN_GAS_OK;
    }
    return PCAN_GAS_READ_ONLY;
}

uint16_t pcan_gas_read_block(const pcan_gas_map_t *map, uint16_t addr,
                             uint16_t *out, uint16_t max)
{
    uint16_t n = 0U;
    while (n < max) {
        /* Останавливаемся на 0xFFFF, чтобы адрес не завернулся в 0. */
        if (pcan_gas_read(map, (uint16_t)(addr + n), &out[n]) != PCAN_GAS_OK) {
            break;
        }
        n++;
        if (((uint32_t)addr + n) > 0xFFFFUL) {
            break;
        }
    }
    return n;
}

uint16_t pcan_gas_write_block(const pcan_gas_map_t *map, uint16_t addr,
                              const uint16_t *in, uint16_t count)
{
    uint16_t n = 0U;
    while (n < count) {
        if (pcan_gas_write(map, (uint16_t)(addr + n), in[n]) != PCAN_GAS_OK) {
            break;
        }
        n++;
        if (((uint32_t)addr + n) > 0xFFFFUL) {
            break;
        }
    }
    return n;
}

uint16_t pcan_gas_frame_to_regs(const pcan_frame_t *frame, uint16_t *out,
                                uint16_t max)
{
    uint16_t n = 0U;
    for (uint8_t i = 0U; (uint8_t)(i + 1U) < frame->dlc; i = (uint8_t)(i + 2U)) {
        if (n >= max) {
            break;
        }
        out[n++] = (uint16_t)((uint16_t)frame->data[i] |
                              ((uint16_t)frame->data[i + 1U] << 8));
    }
    return n;
}

void pcan_gas_regs_to_frame(pcan_frame_t *frame, uint32_t base_id,
                            uint16_t addr, const uint16_t *regs, uint16_t count)
{
    if (count > PCAN_GAS_REGS_PER_FRAME) {
        count = PCAN_GAS_REGS_PER_FRAME;
    }

    pcan_id_t id;
    pcan_id_unpack(base_id, &id);
    id.msg_type = PCAN_MSG_GAS;
    id.msg_body = addr;

    frame->id = pcan_id_pack(&id);
    frame->flags = PCAN_FLAG_IDE;
    frame->seq = 0U;
    frame->dlc = (uint8_t)(count * 2U);
    for (uint16_t i = 0U; i < count; i++) {
        frame->data[i * 2U] = (uint8_t)(regs[i] & 0xFFU);
        frame->data[(i * 2U) + 1U] = (uint8_t)(regs[i] >> 8);
    }
    for (uint8_t i = frame->dlc; i < PCAN_DATA_MAX; i++) {
        frame->data[i] = 0U;
    }
}

bool pcan_gas_handle(const pcan_gas_map_t *map, const pcan_frame_t *req,
                     pcan_frame_t *rsp)
{
    pcan_id_t id;
    pcan_id_unpack(req->id, &id);
    if (id.msg_type != (uint8_t)PCAN_MSG_GAS) {
        return false;
    }

    if (req->dlc == 0U) {
        uint16_t regs[PCAN_GAS_REGS_PER_FRAME];
        uint16_t n = pcan_gas_read_block(map, id.msg_body, regs,
                                         PCAN_GAS_REGS_PER_FRAME);
        if (n == 0U) {
            return false;   /* адреса нет в карте - отвечать нечем */
        }
        pcan_gas_regs_to_frame(rsp, req->id, id.msg_body, regs, n);
        pcan_id_unpack(rsp->id, &id);
        id.route = PCAN_ROUTE_FROM_DEVICE;
        rsp->id = pcan_id_pack(&id);
        rsp->flags = (uint8_t)(req->flags & PCAN_FLAG_IDE);
        return true;
    }

    uint16_t regs[PCAN_GAS_REGS_PER_FRAME];
    uint16_t n = pcan_gas_frame_to_regs(req, regs, PCAN_GAS_REGS_PER_FRAME);
    (void)pcan_gas_write_block(map, id.msg_body, regs, n);
    return false;
}