#include "ad_usb_cdc.h"

#include <stddef.h>
#include <string.h>

#include "ad_project_config.h"
#include "main.h"
#include "simulink_interface.h"

#ifndef AD_USB_CDC_ENABLE
#define AD_USB_CDC_ENABLE 1
#endif

#define AD_USB_CDC_EP0_SIZE             64U
#define AD_USB_CDC_EP0_BUFFER_BYTES     128U
#define AD_USB_CDC_DATA_EP_SIZE         64U
#define AD_USB_CDC_CMD_EP_SIZE          8U
#define AD_USB_CDC_EP_IN                0x81U
#define AD_USB_CDC_EP_OUT               0x01U
#define AD_USB_CDC_EP_CMD               0x82U

#define USB_REQ_GET_STATUS              0x00U
#define USB_REQ_CLEAR_FEATURE           0x01U
#define USB_REQ_SET_FEATURE             0x03U
#define USB_REQ_SET_ADDRESS             0x05U
#define USB_REQ_GET_DESCRIPTOR          0x06U
#define USB_REQ_GET_CONFIGURATION       0x08U
#define USB_REQ_SET_CONFIGURATION       0x09U

#define USB_REQ_TYPE_MASK               0x60U
#define USB_REQ_TYPE_STANDARD           0x00U
#define USB_REQ_TYPE_CLASS              0x20U
#define USB_REQ_DIR_IN                  0x80U

#define USB_DESC_TYPE_DEVICE            0x01U
#define USB_DESC_TYPE_CONFIGURATION     0x02U
#define USB_DESC_TYPE_STRING            0x03U

#define CDC_REQ_SET_LINE_CODING         0x20U
#define CDC_REQ_GET_LINE_CODING         0x21U
#define CDC_REQ_SET_CONTROL_LINE_STATE  0x22U

typedef struct
{
    uint8_t bm_request_type;
    uint8_t b_request;
    uint16_t w_value;
    uint16_t w_index;
    uint16_t w_length;
} AD_USB_SetupPacket_t;

PCD_HandleTypeDef hpcd_USB_FS;

static AD_USB_CDC_State_t g_usb_state;
static uint8_t g_configuration;
static uint8_t g_pending_address;
static uint8_t g_address_pending;
static uint8_t g_ep0_out_request;
static uint16_t g_control_line_state;
static uint8_t g_line_coding[7] = { 0x00U, 0xC2U, 0x01U, 0x00U, 0x00U, 0x00U, 0x08U };
static uint8_t g_ep0_tx_buffer[AD_USB_CDC_EP0_BUFFER_BYTES];
static uint8_t g_string_descriptor[64];
static uint8_t g_rx_buffer[AD_USB_CDC_DATA_EP_SIZE];
static uint8_t g_tx_buffer[SIMULINK_TELEMETRY_MAX_BYTES];
static uint16_t g_tx_size;

static const uint8_t g_device_descriptor[] =
{
    0x12U, USB_DESC_TYPE_DEVICE,
    0x00U, 0x02U,
    0x02U, 0x00U, 0x00U,
    AD_USB_CDC_EP0_SIZE,
    0x83U, 0x04U,
    0x40U, 0x57U,
    0x00U, 0x02U,
    0x01U, 0x02U, 0x03U,
    0x01U
};

static const uint8_t g_configuration_descriptor[] =
{
    0x09U, USB_DESC_TYPE_CONFIGURATION, 0x43U, 0x00U, 0x02U, 0x01U, 0x00U, 0x80U, 0x32U,

    0x09U, 0x04U, 0x00U, 0x00U, 0x01U, 0x02U, 0x02U, 0x01U, 0x00U,
    0x05U, 0x24U, 0x00U, 0x10U, 0x01U,
    0x05U, 0x24U, 0x01U, 0x00U, 0x01U,
    0x04U, 0x24U, 0x02U, 0x02U,
    0x05U, 0x24U, 0x06U, 0x00U, 0x01U,
    0x07U, 0x05U, AD_USB_CDC_EP_CMD, 0x03U, AD_USB_CDC_CMD_EP_SIZE, 0x00U, 0xFFU,

    0x09U, 0x04U, 0x01U, 0x00U, 0x02U, 0x0AU, 0x00U, 0x00U, 0x00U,
    0x07U, 0x05U, AD_USB_CDC_EP_OUT, 0x02U, AD_USB_CDC_DATA_EP_SIZE, 0x00U, 0x00U,
    0x07U, 0x05U, AD_USB_CDC_EP_IN, 0x02U, AD_USB_CDC_DATA_EP_SIZE, 0x00U, 0x00U
};

static void update_status_flags(void)
{
    uint32_t flags = 0UL;

    if (g_usb_state.initialized != 0U) {
        flags |= AD_USB_CDC_STATUS_INITIALIZED;
    }
    if (g_usb_state.configured != 0U) {
        flags |= AD_USB_CDC_STATUS_CONFIGURED;
    }
    if (g_usb_state.tx_busy != 0U) {
        flags |= AD_USB_CDC_STATUS_TX_BUSY;
    }
    if ((g_usb_state.status_flags & AD_USB_CDC_STATUS_ENUM_ERROR) != 0UL) {
        flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
    }
    if ((g_usb_state.status_flags & AD_USB_CDC_STATUS_TX_DROPPED) != 0UL) {
        flags |= AD_USB_CDC_STATUS_TX_DROPPED;
    }

    g_usb_state.status_flags = flags;
}

static uint16_t min_u16(uint16_t a, uint16_t b)
{
    return (a < b) ? a : b;
}

static void parse_setup(PCD_HandleTypeDef *hpcd, AD_USB_SetupPacket_t *setup)
{
    const uint8_t *raw = (const uint8_t *)hpcd->Setup;

    setup->bm_request_type = raw[0];
    setup->b_request = raw[1];
    setup->w_value = (uint16_t)raw[2] | ((uint16_t)raw[3] << 8);
    setup->w_index = (uint16_t)raw[4] | ((uint16_t)raw[5] << 8);
    setup->w_length = (uint16_t)raw[6] | ((uint16_t)raw[7] << 8);
}

static void ep0_stall(void)
{
    (void)HAL_PCD_EP_SetStall(&hpcd_USB_FS, 0x80U);
    (void)HAL_PCD_EP_SetStall(&hpcd_USB_FS, 0x00U);
    g_usb_state.status_flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
    update_status_flags();
}

static void ep0_send(const uint8_t *data, uint16_t size, uint16_t requested)
{
    uint16_t send_size = min_u16(size, requested);

    if (send_size > sizeof(g_ep0_tx_buffer)) {
        send_size = sizeof(g_ep0_tx_buffer);
    }
    if ((send_size > 0U) && (data != 0)) {
        memcpy(g_ep0_tx_buffer, data, send_size);
    }

    (void)HAL_PCD_EP_Transmit(&hpcd_USB_FS, 0x80U, g_ep0_tx_buffer, send_size);
}

static void ep0_send_zlp(void)
{
    (void)HAL_PCD_EP_Transmit(&hpcd_USB_FS, 0x80U, g_ep0_tx_buffer, 0U);
}

static uint16_t make_string_descriptor(const char *text)
{
    uint16_t len = 0U;
    uint16_t i;

    while ((text[len] != '\0') && (len < 31U)) {
        len++;
    }

    g_string_descriptor[0] = (uint8_t)(2U + (len * 2U));
    g_string_descriptor[1] = USB_DESC_TYPE_STRING;
    for (i = 0U; i < len; i++) {
        g_string_descriptor[2U + (i * 2U)] = (uint8_t)text[i];
        g_string_descriptor[3U + (i * 2U)] = 0U;
    }

    return (uint16_t)g_string_descriptor[0];
}

static void send_string_descriptor(uint8_t index, uint16_t requested)
{
    uint16_t size;

    if (index == 0U) {
        static const uint8_t lang_id[] = { 0x04U, USB_DESC_TYPE_STRING, 0x09U, 0x04U };
        ep0_send(lang_id, sizeof(lang_id), requested);
        return;
    }

    switch (index) {
    case 1U:
        size = make_string_descriptor("SET");
        break;
    case 2U:
        size = make_string_descriptor("AD Telemetry CDC");
        break;
    case 3U:
        size = make_string_descriptor("AD0001");
        break;
    default:
        ep0_stall();
        return;
    }

    ep0_send(g_string_descriptor, size, requested);
}

static void open_cdc_endpoints(void)
{
    (void)HAL_PCD_EP_Open(&hpcd_USB_FS, AD_USB_CDC_EP_CMD, AD_USB_CDC_CMD_EP_SIZE, EP_TYPE_INTR);
    (void)HAL_PCD_EP_Open(&hpcd_USB_FS, AD_USB_CDC_EP_OUT, AD_USB_CDC_DATA_EP_SIZE, EP_TYPE_BULK);
    (void)HAL_PCD_EP_Open(&hpcd_USB_FS, AD_USB_CDC_EP_IN, AD_USB_CDC_DATA_EP_SIZE, EP_TYPE_BULK);
    (void)HAL_PCD_EP_Receive(&hpcd_USB_FS, AD_USB_CDC_EP_OUT, g_rx_buffer, sizeof(g_rx_buffer));
}

static void close_cdc_endpoints(void)
{
    (void)HAL_PCD_EP_Close(&hpcd_USB_FS, AD_USB_CDC_EP_CMD);
    (void)HAL_PCD_EP_Close(&hpcd_USB_FS, AD_USB_CDC_EP_OUT);
    (void)HAL_PCD_EP_Close(&hpcd_USB_FS, AD_USB_CDC_EP_IN);
}

static void handle_standard_request(const AD_USB_SetupPacket_t *setup)
{
    uint8_t descriptor_type;
    uint8_t descriptor_index;
    uint8_t status[2] = { 0U, 0U };

    switch (setup->b_request) {
    case USB_REQ_GET_DESCRIPTOR:
        descriptor_type = (uint8_t)(setup->w_value >> 8);
        descriptor_index = (uint8_t)(setup->w_value & 0xFFU);
        if (descriptor_type == USB_DESC_TYPE_DEVICE) {
            ep0_send(g_device_descriptor, sizeof(g_device_descriptor), setup->w_length);
        } else if (descriptor_type == USB_DESC_TYPE_CONFIGURATION) {
            ep0_send(g_configuration_descriptor, sizeof(g_configuration_descriptor), setup->w_length);
        } else if (descriptor_type == USB_DESC_TYPE_STRING) {
            send_string_descriptor(descriptor_index, setup->w_length);
        } else {
            ep0_stall();
        }
        break;

    case USB_REQ_SET_ADDRESS:
        g_pending_address = (uint8_t)(setup->w_value & 0x7FU);
        g_address_pending = 1U;
        ep0_send_zlp();
        break;

    case USB_REQ_SET_CONFIGURATION:
        g_configuration = (uint8_t)(setup->w_value & 0xFFU);
        if (g_configuration != 0U) {
            open_cdc_endpoints();
            g_usb_state.configured = 1U;
        } else {
            close_cdc_endpoints();
            g_usb_state.configured = 0U;
        }
        update_status_flags();
        ep0_send_zlp();
        break;

    case USB_REQ_GET_CONFIGURATION:
        ep0_send(&g_configuration, 1U, setup->w_length);
        break;

    case USB_REQ_GET_STATUS:
        ep0_send(status, sizeof(status), setup->w_length);
        break;

    case USB_REQ_CLEAR_FEATURE:
    case USB_REQ_SET_FEATURE:
        ep0_send_zlp();
        break;

    default:
        ep0_stall();
        break;
    }
}

static void handle_class_request(const AD_USB_SetupPacket_t *setup)
{
    switch (setup->b_request) {
    case CDC_REQ_SET_LINE_CODING:
        if (((setup->bm_request_type & USB_REQ_DIR_IN) == 0U) && (setup->w_length == sizeof(g_line_coding))) {
            g_ep0_out_request = CDC_REQ_SET_LINE_CODING;
            (void)HAL_PCD_EP_Receive(&hpcd_USB_FS, 0x00U, g_line_coding, sizeof(g_line_coding));
        } else {
            ep0_stall();
        }
        break;

    case CDC_REQ_GET_LINE_CODING:
        ep0_send(g_line_coding, sizeof(g_line_coding), setup->w_length);
        break;

    case CDC_REQ_SET_CONTROL_LINE_STATE:
        g_control_line_state = setup->w_value;
        ep0_send_zlp();
        break;

    default:
        ep0_stall();
        break;
    }
}

void AD_USB_CDC_Init(void)
{
#if AD_USB_CDC_ENABLE != 0
    uint32_t tickstart;

    memset(&g_usb_state, 0, sizeof(g_usb_state));
    memset(&hpcd_USB_FS, 0, sizeof(hpcd_USB_FS));
    memset(g_ep0_tx_buffer, 0, sizeof(g_ep0_tx_buffer));
    memset(g_rx_buffer, 0, sizeof(g_rx_buffer));
    memset(g_tx_buffer, 0, sizeof(g_tx_buffer));
    g_configuration = 0U;
    g_pending_address = 0U;
    g_address_pending = 0U;
    g_ep0_out_request = 0U;
    g_control_line_state = 0U;
    g_tx_size = 0U;

    __HAL_RCC_HSI48_ENABLE();
    tickstart = HAL_GetTick();
    while (__HAL_RCC_GET_FLAG(RCC_FLAG_HSI48RDY) == RESET) {
        if ((HAL_GetTick() - tickstart) > 10UL) {
            g_usb_state.last_error = 1UL;
            g_usb_state.status_flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
            update_status_flags();
            return;
        }
    }
    __HAL_RCC_USB_CONFIG(RCC_USBCLKSOURCE_HSI48);

    hpcd_USB_FS.Instance = USB;
    hpcd_USB_FS.Init.dev_endpoints = 8U;
    hpcd_USB_FS.Init.speed = PCD_SPEED_FULL;
    hpcd_USB_FS.Init.ep0_mps = PCD_EP0MPS_64;
    hpcd_USB_FS.Init.phy_itface = PCD_PHY_EMBEDDED;
    hpcd_USB_FS.Init.Sof_enable = DISABLE;
    hpcd_USB_FS.Init.low_power_enable = DISABLE;
    hpcd_USB_FS.Init.lpm_enable = DISABLE;
    hpcd_USB_FS.Init.battery_charging_enable = DISABLE;

    if (HAL_PCD_Init(&hpcd_USB_FS) != HAL_OK) {
        g_usb_state.last_error = 2UL;
        g_usb_state.status_flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
        update_status_flags();
        return;
    }

    (void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, 0x00U, PCD_SNG_BUF, 0x18U);
    (void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, 0x80U, PCD_SNG_BUF, 0x58U);
    (void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, AD_USB_CDC_EP_OUT, PCD_SNG_BUF, 0x98U);
    (void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, AD_USB_CDC_EP_IN, PCD_SNG_BUF, 0xD8U);
    (void)HAL_PCDEx_PMAConfig(&hpcd_USB_FS, AD_USB_CDC_EP_CMD, PCD_SNG_BUF, 0x118U);

    if (HAL_PCD_Start(&hpcd_USB_FS) != HAL_OK) {
        g_usb_state.last_error = 3UL;
        g_usb_state.status_flags |= AD_USB_CDC_STATUS_ENUM_ERROR;
        update_status_flags();
        return;
    }
    (void)HAL_PCD_DevConnect(&hpcd_USB_FS);

    g_usb_state.initialized = 1U;
    update_status_flags();
#else
    memset(&g_usb_state, 0, sizeof(g_usb_state));
#endif
}

void AD_USB_CDC_Loop(void)
{
    update_status_flags();
}

uint8_t AD_USB_CDC_WritePacket(const uint8_t *data, uint16_t size)
{
#if AD_USB_CDC_ENABLE != 0
    if ((data == 0) ||
        (size == 0U) ||
        (size > SIMULINK_TELEMETRY_MAX_BYTES) ||
        (g_usb_state.configured == 0U) ||
        (g_usb_state.tx_busy != 0U)) {
        g_usb_state.dropped_packets++;
        g_usb_state.status_flags |= AD_USB_CDC_STATUS_TX_DROPPED;
        update_status_flags();
        return 0U;
    }

    memcpy(g_tx_buffer, data, size);
    g_tx_size = size;
    g_usb_state.tx_busy = 1U;
    g_usb_state.status_flags &= ~AD_USB_CDC_STATUS_TX_DROPPED;
    update_status_flags();

    if (HAL_PCD_EP_Transmit(&hpcd_USB_FS, AD_USB_CDC_EP_IN, g_tx_buffer, size) != HAL_OK) {
        g_usb_state.tx_busy = 0U;
        g_usb_state.dropped_packets++;
        g_usb_state.status_flags |= AD_USB_CDC_STATUS_TX_DROPPED;
        update_status_flags();
        return 0U;
    }

    return 1U;
#else
    (void)data;
    (void)size;
    return 0U;
#endif
}

uint8_t AD_USB_CDC_IsConfigured(void)
{
    return g_usb_state.configured;
}

uint8_t AD_USB_CDC_IsTxBusy(void)
{
    return g_usb_state.tx_busy;
}

const AD_USB_CDC_State_t* AD_USB_CDC_GetState(void)
{
    update_status_flags();
    return &g_usb_state;
}

void AD_USB_CDC_IRQHandler(void)
{
#if AD_USB_CDC_ENABLE != 0
    HAL_PCD_IRQHandler(&hpcd_USB_FS);
#endif
}

void HAL_PCD_SetupStageCallback(PCD_HandleTypeDef *hpcd)
{
    AD_USB_SetupPacket_t setup;

    if (hpcd != &hpcd_USB_FS) {
        return;
    }

    parse_setup(hpcd, &setup);
    g_usb_state.setup_packets++;

    if ((setup.bm_request_type & USB_REQ_TYPE_MASK) == USB_REQ_TYPE_STANDARD) {
        handle_standard_request(&setup);
    } else if ((setup.bm_request_type & USB_REQ_TYPE_MASK) == USB_REQ_TYPE_CLASS) {
        handle_class_request(&setup);
    } else {
        ep0_stall();
    }
}

void HAL_PCD_ResetCallback(PCD_HandleTypeDef *hpcd)
{
    if (hpcd != &hpcd_USB_FS) {
        return;
    }

    g_configuration = 0U;
    g_pending_address = 0U;
    g_address_pending = 0U;
    g_ep0_out_request = 0U;
    g_usb_state.configured = 0U;
    g_usb_state.tx_busy = 0U;
    g_tx_size = 0U;

    (void)HAL_PCD_SetAddress(hpcd, 0U);
    (void)HAL_PCD_EP_Open(hpcd, 0x00U, AD_USB_CDC_EP0_SIZE, EP_TYPE_CTRL);
    (void)HAL_PCD_EP_Open(hpcd, 0x80U, AD_USB_CDC_EP0_SIZE, EP_TYPE_CTRL);
    update_status_flags();
}

void HAL_PCD_DataInStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
{
    if (hpcd != &hpcd_USB_FS) {
        return;
    }

    if (epnum == 0U) {
        if (g_address_pending != 0U) {
            (void)HAL_PCD_SetAddress(hpcd, g_pending_address);
            g_address_pending = 0U;
        }
    } else if (epnum == (AD_USB_CDC_EP_IN & 0x7FU)) {
        g_usb_state.tx_busy = 0U;
        g_usb_state.tx_packets++;
        g_usb_state.tx_bytes += g_tx_size;
        g_tx_size = 0U;
        update_status_flags();
    } else {
        update_status_flags();
    }
}

void HAL_PCD_DataOutStageCallback(PCD_HandleTypeDef *hpcd, uint8_t epnum)
{
    uint32_t rx_count;

    if (hpcd != &hpcd_USB_FS) {
        return;
    }

    if (epnum == 0U) {
        if (g_ep0_out_request == CDC_REQ_SET_LINE_CODING) {
            g_ep0_out_request = 0U;
            ep0_send_zlp();
        }
        return;
    }

    if (epnum == (AD_USB_CDC_EP_OUT & 0x7FU)) {
        rx_count = HAL_PCD_EP_GetRxCount(hpcd, AD_USB_CDC_EP_OUT);
        g_usb_state.rx_bytes += rx_count;
        (void)HAL_PCD_EP_Receive(hpcd, AD_USB_CDC_EP_OUT, g_rx_buffer, sizeof(g_rx_buffer));
    }
}

void HAL_PCD_SuspendCallback(PCD_HandleTypeDef *hpcd)
{
    (void)hpcd;
}

void HAL_PCD_ResumeCallback(PCD_HandleTypeDef *hpcd)
{
    (void)hpcd;
}

void HAL_PCD_ConnectCallback(PCD_HandleTypeDef *hpcd)
{
    (void)hpcd;
}

void HAL_PCD_DisconnectCallback(PCD_HandleTypeDef *hpcd)
{
    if (hpcd == &hpcd_USB_FS) {
        g_usb_state.configured = 0U;
        g_usb_state.tx_busy = 0U;
        update_status_flags();
    }
}
