#include "pcan_boot.h"

#include <string.h>

#define PCAN_BOOT_ID_MASK 0x1FFFFFFFUL

static uint16_t get_u16(const uint8_t *p)
{
    return (uint16_t)((uint16_t)p[0] | ((uint16_t)p[1] << 8));
}

static uint32_t get_u32(const uint8_t *p)
{
    return (uint32_t)p[0]
         | ((uint32_t)p[1] << 8)
         | ((uint32_t)p[2] << 16)
         | ((uint32_t)p[3] << 24);
}

static void put_u16(uint8_t *p, uint16_t value)
{
    p[0] = (uint8_t)value;
    p[1] = (uint8_t)(value >> 8);
}

static void put_u32(uint8_t *p, uint32_t value)
{
    p[0] = (uint8_t)value;
    p[1] = (uint8_t)(value >> 8);
    p[2] = (uint8_t)(value >> 16);
    p[3] = (uint8_t)(value >> 24);
}

static uint32_t crc32_update(uint32_t crc, const uint8_t *data, size_t length)
{
    size_t i;
    crc = ~crc;
    for (i = 0U; i < length; ++i) {
        uint8_t bit;
        crc ^= data[i];
        for (bit = 0U; bit < 8U; ++bit) {
            crc = (crc >> 1) ^ ((crc & 1U) != 0U ? 0xEDB88320UL : 0U);
        }
    }
    return ~crc;
}

uint32_t pcan_boot_make_id(uint8_t priority, uint8_t route,
                           uint8_t device_type, uint8_t device_id,
                           uint8_t msg_type, uint16_t msg_body)
{
    return ((((uint32_t)priority & 1U) << 28)
          | (((uint32_t)route & 1U) << 27)
          | (((uint32_t)device_type & 7U) << 24)
          | (((uint32_t)device_id & 15U) << 20)
          | (((uint32_t)msg_type & 15U) << 16)
          | msg_body) & PCAN_BOOT_ID_MASK;
}

uint8_t pcan_boot_id_route(uint32_t can_id) { return (uint8_t)((can_id >> 27) & 1U); }
uint8_t pcan_boot_id_device_type(uint32_t can_id) { return (uint8_t)((can_id >> 24) & 7U); }
uint8_t pcan_boot_id_device_id(uint32_t can_id) { return (uint8_t)((can_id >> 20) & 15U); }
uint8_t pcan_boot_id_msg_type(uint32_t can_id) { return (uint8_t)((can_id >> 16) & 15U); }
uint16_t pcan_boot_id_msg_body(uint32_t can_id) { return (uint16_t)can_id; }

static bool send_frame(pcan_boot_t *boot, uint8_t msg_type, uint16_t body,
                       const uint8_t *data, uint8_t dlc)
{
    uint32_t id = pcan_boot_make_id(1U, PCAN_BOOT_ROUTE_FROM_DEVICE,
                                    boot->config.device_type,
                                    boot->config.device_id,
                                    msg_type, body);
    return boot->port.send(boot->port_user, id, data, dlc);
}

static bool send_status(pcan_boot_t *boot, uint8_t command, uint8_t status)
{
    uint8_t data[8];
    uint16_t body = (uint16_t)(((uint16_t)boot->session_id << 8) | command);
    data[0] = status;
    data[1] = boot->target_slot;
    put_u16(&data[2], boot->next_block);
    put_u32(&data[4], boot->running_crc32);
    boot->last_status = status;
    return send_frame(boot, PCAN_BOOT_MSG_STATUS, body, data, 8U);
}

static bool send_identity(pcan_boot_t *boot)
{
    uint8_t data[8];
    put_u16(&data[0], boot->config.product_type);
    data[2] = boot->config.hardware_revision;
    data[3] = PCAN_BOOT_VERSION;
    put_u32(&data[4], boot->config.firmware_version);
    return send_frame(boot, PCAN_BOOT_MSG_DISCOVERY, 1U, data, 8U);
}

void pcan_boot_abort(pcan_boot_t *boot)
{
    if (boot == NULL) {
        return;
    }
    (void)memset(&boot->manifest, 0, sizeof(boot->manifest));
    boot->state = PCAN_BOOT_STATE_IDLE;
    boot->running_crc32 = 0U;
    boot->bytes_received = 0U;
    boot->next_block = 0U;
    boot->session_id = 0U;
    boot->target_slot = PCAN_BOOT_SLOT_NONE;
    boot->last_status = PCAN_BOOT_STATUS_OK;
    boot->have_image = false;
    boot->have_compat = false;
}

bool pcan_boot_init(pcan_boot_t *boot, const pcan_boot_config_t *config,
                    const pcan_boot_port_t *port, void *port_user)
{
    uint8_t slot_count;
    if ((boot == NULL) || (config == NULL) || (port == NULL)
            || (config->device_type > 7U) || (config->device_id > 15U)
            || (config->active_slot > 1U)
            || (port->send == NULL) || (port->erase_slot == NULL)
            || (port->write_slot == NULL)) {
        return false;
    }
    slot_count = config->slot_count == 0U ? 2U : config->slot_count;
    if ((slot_count > 2U) || (config->active_slot >= slot_count)
            || ((slot_count == 2U) && (port->set_pending_slot == NULL))) {
        return false;
    }
    (void)memset(boot, 0, sizeof(*boot));
    boot->config = *config;
    boot->port = *port;
    boot->port_user = port_user;
    boot->config.slot_count = slot_count;
    if (boot->config.max_image_size == 0U) {
        boot->config.max_image_size = PCAN_BOOT_SLOT_SIZE;
    }
    if (boot->config.ack_window == 0U) {
        boot->config.ack_window = 1U;
    }
    pcan_boot_abort(boot);
    return true;
}

static bool metadata_complete(pcan_boot_t *boot, uint8_t command)
{
    if (!boot->have_image || !boot->have_compat) {
        boot->state = PCAN_BOOT_STATE_METADATA;
        return send_status(boot, command, PCAN_BOOT_STATUS_OK);
    }
    if ((boot->manifest.image_size == 0U)
            || (boot->manifest.image_size > boot->config.max_image_size)) {
        boot->state = PCAN_BOOT_STATE_FAILED;
        return send_status(boot, command, PCAN_BOOT_STATUS_INVALID_SIZE);
    }
    if ((boot->manifest.product_type != boot->config.product_type)
            || (boot->config.hardware_revision < boot->manifest.hardware_revision_min)
            || (boot->config.hardware_revision > boot->manifest.hardware_revision_max)) {
        boot->state = PCAN_BOOT_STATE_FAILED;
        return send_status(boot, command, PCAN_BOOT_STATUS_WRONG_HARDWARE);
    }
    if ((boot->port.authorize != NULL)
            && !boot->port.authorize(boot->port_user, &boot->manifest)) {
        boot->state = PCAN_BOOT_STATE_FAILED;
        return send_status(boot, command, PCAN_BOOT_STATUS_SIGNATURE_ERROR);
    }
    boot->target_slot = boot->config.slot_count == 1U
                      ? boot->config.active_slot
                      : (uint8_t)(boot->config.active_slot ^ 1U);
    boot->state = PCAN_BOOT_STATE_READY_TO_ERASE;
    return send_status(boot, command, PCAN_BOOT_STATUS_OK);
}

static bool process_control(pcan_boot_t *boot, uint16_t body,
                            const uint8_t *data, uint8_t dlc)
{
    uint8_t session = (uint8_t)(body >> 8);
    uint8_t command = (uint8_t)body;

    if (command == PCAN_BOOT_CMD_IDENTIFY) {
        return send_identity(boot);
    }
    if (command == PCAN_BOOT_CMD_ENTER_BOOT) {
        if (session == 0U) {
            return send_status(boot, command, PCAN_BOOT_STATUS_SESSION_ERROR);
        }
        pcan_boot_abort(boot);
        boot->session_id = session;
        boot->state = PCAN_BOOT_STATE_METADATA;
        return send_status(boot, command, PCAN_BOOT_STATUS_OK);
    }
    if ((session == 0U) || (session != boot->session_id)) {
        return send_status(boot, command, PCAN_BOOT_STATUS_SESSION_ERROR);
    }

    switch (command) {
    case PCAN_BOOT_CMD_BEGIN_IMAGE:
        if ((dlc != 8U) || (boot->state != PCAN_BOOT_STATE_METADATA)) {
            return send_status(boot, command, PCAN_BOOT_STATUS_INVALID_STATE);
        }
        boot->manifest.image_size = get_u32(&data[0]);
        boot->manifest.image_crc32 = get_u32(&data[4]);
        boot->have_image = true;
        return metadata_complete(boot, command);

    case PCAN_BOOT_CMD_BEGIN_COMPAT:
        if ((dlc != 8U) || (boot->state != PCAN_BOOT_STATE_METADATA)) {
            return send_status(boot, command, PCAN_BOOT_STATUS_INVALID_STATE);
        }
        boot->manifest.product_type = get_u16(&data[0]);
        boot->manifest.hardware_revision_min = data[2];
        boot->manifest.hardware_revision_max = data[3];
        boot->manifest.firmware_version = get_u32(&data[4]);
        boot->have_compat = true;
        return metadata_complete(boot, command);

    case PCAN_BOOT_CMD_ERASE:
        if (boot->state != PCAN_BOOT_STATE_READY_TO_ERASE) {
            return send_status(boot, command, PCAN_BOOT_STATUS_INVALID_STATE);
        }
        if (!boot->port.erase_slot(boot->port_user, boot->target_slot)) {
            boot->state = PCAN_BOOT_STATE_FAILED;
            return send_status(boot, command, PCAN_BOOT_STATUS_FLASH_ERROR);
        }
        boot->running_crc32 = 0U;
        boot->bytes_received = 0U;
        boot->next_block = 0U;
        boot->state = PCAN_BOOT_STATE_RECEIVING;
        return send_status(boot, command, PCAN_BOOT_STATUS_OK);

    case PCAN_BOOT_CMD_VERIFY:
        if ((boot->state != PCAN_BOOT_STATE_RECEIVING)
                || (boot->bytes_received != boot->manifest.image_size)) {
            return send_status(boot, command, PCAN_BOOT_STATUS_INVALID_STATE);
        }
        if (boot->running_crc32 != boot->manifest.image_crc32) {
            boot->state = PCAN_BOOT_STATE_FAILED;
            return send_status(boot, command, PCAN_BOOT_STATUS_CRC_ERROR);
        }
        if ((boot->port.verify_image != NULL)
                && !boot->port.verify_image(boot->port_user, boot->target_slot,
                                            &boot->manifest)) {
            boot->state = PCAN_BOOT_STATE_FAILED;
            return send_status(boot, command, PCAN_BOOT_STATUS_SIGNATURE_ERROR);
        }
        boot->state = PCAN_BOOT_STATE_VERIFIED;
        return send_status(boot, command, PCAN_BOOT_STATUS_OK);

    case PCAN_BOOT_CMD_COMMIT:
        if (boot->state != PCAN_BOOT_STATE_VERIFIED) {
            return send_status(boot, command, PCAN_BOOT_STATUS_INVALID_STATE);
        }
        if ((boot->config.slot_count == 2U)
                && !boot->port.set_pending_slot(boot->port_user, boot->target_slot,
                                                &boot->manifest)) {
            return send_status(boot, command, PCAN_BOOT_STATUS_FLASH_ERROR);
        }
        return send_status(boot, command, PCAN_BOOT_STATUS_OK);

    case PCAN_BOOT_CMD_CONFIRM:
        if ((boot->port.confirm_running_slot == NULL)
                || !boot->port.confirm_running_slot(boot->port_user)) {
            return send_status(boot, command, PCAN_BOOT_STATUS_FLASH_ERROR);
        }
        return send_status(boot, command, PCAN_BOOT_STATUS_OK);

    case PCAN_BOOT_CMD_QUERY_PROGRESS:
        return send_status(boot, command, boot->last_status);

    case PCAN_BOOT_CMD_ABORT:
        pcan_boot_abort(boot);
        boot->session_id = session;
        return send_status(boot, command, PCAN_BOOT_STATUS_OK);

    case PCAN_BOOT_CMD_REBOOT:
        (void)send_status(boot, command, PCAN_BOOT_STATUS_OK);
        if (boot->port.reboot != NULL) {
            boot->port.reboot(boot->port_user);
        }
        return true;

    default:
        return send_status(boot, command, PCAN_BOOT_STATUS_INVALID_COMMAND);
    }
}

static bool process_data(pcan_boot_t *boot, uint8_t msg_type, uint16_t block,
                         const uint8_t *data, uint8_t dlc)
{
    uint8_t expected_type;
    uint8_t write_length;
    uint32_t remaining;

    if (boot->state != PCAN_BOOT_STATE_RECEIVING) {
        return send_status(boot, 0U, PCAN_BOOT_STATUS_INVALID_STATE);
    }
    expected_type = boot->target_slot == 0U ? PCAN_BOOT_MSG_DATA_A
                                            : PCAN_BOOT_MSG_DATA_B;
    if (msg_type != expected_type) {
        return send_status(boot, 0U, PCAN_BOOT_STATUS_INVALID_STATE);
    }
    if ((block != boot->next_block) || (dlc != PCAN_BOOT_BLOCK_SIZE)) {
        return send_status(boot, 0U, PCAN_BOOT_STATUS_SEQUENCE_ERROR);
    }
    remaining = boot->manifest.image_size - boot->bytes_received;
    write_length = remaining < PCAN_BOOT_BLOCK_SIZE ? (uint8_t)remaining
                                                    : PCAN_BOOT_BLOCK_SIZE;
    if ((write_length == 0U)
            || !boot->port.write_slot(boot->port_user, boot->target_slot,
                                      boot->bytes_received, data, write_length)) {
        boot->state = PCAN_BOOT_STATE_FAILED;
        return send_status(boot, 0U, PCAN_BOOT_STATUS_FLASH_ERROR);
    }
    boot->running_crc32 = crc32_update(boot->running_crc32, data, write_length);
    boot->bytes_received += write_length;
    boot->next_block++;

    if ((boot->bytes_received == boot->manifest.image_size)
            || ((boot->next_block % boot->config.ack_window) == 0U)) {
        return send_status(boot, 0U, PCAN_BOOT_STATUS_OK);
    }
    return true;
}

bool pcan_boot_process(pcan_boot_t *boot, uint32_t can_id,
                       const uint8_t *data, uint8_t dlc)
{
    uint8_t msg_type;
    if ((boot == NULL) || (dlc > 8U) || ((dlc != 0U) && (data == NULL))) {
        return false;
    }
    msg_type = pcan_boot_id_msg_type(can_id);
    if ((msg_type < PCAN_BOOT_MSG_CONTROL) || (msg_type > PCAN_BOOT_MSG_DISCOVERY)) {
        return false;
    }
    if ((pcan_boot_id_route(can_id) != PCAN_BOOT_ROUTE_FROM_PM)
            || (pcan_boot_id_device_type(can_id) != boot->config.device_type)
            || (pcan_boot_id_device_id(can_id) != boot->config.device_id)) {
        return false;
    }
    if (msg_type == PCAN_BOOT_MSG_CONTROL) {
        return process_control(boot, pcan_boot_id_msg_body(can_id), data, dlc);
    }
    if ((msg_type == PCAN_BOOT_MSG_DATA_A) || (msg_type == PCAN_BOOT_MSG_DATA_B)) {
        return process_data(boot, msg_type, pcan_boot_id_msg_body(can_id), data, dlc);
    }
    return false;
}