#include "fr30xx.h"

#include "FreeRTOS.h"

#include "SWD.h"
#include "fdb_app.h"

#include "controller_bootloader.h"

//#define CONTROLLER_CODE_SPLIT

#define HCI_UART                UART0
#define HCI_UART_IRQn           UART0_IRQn

#ifndef CONTROLLER_CODE_SPLIT
#define CODE_EXEC_BASE_ADDR     0x1FFFA000

extern uint8_t CODE_BASE, CODE_END;
#else
#define CODE_EXEC_L_BASE_ADDR       0x1FFF8000
#define CODE_EXEC_H_BASE_ADDR_P1    0x20008700
//#define CODE_EXEC_H_BASE_ADDR_P2    0x20008700

extern uint8_t CODE_L_BASE, CODE_L_END;
extern uint8_t CODE_H_BASE, CODE_H_END;
#endif

#define CONTROLLER_PARAM_DUMMY          0x00
#define CONTROLLER_PARAM_BAUDRATE       0x01
#define CONTROLLER_PARAM_KEY            0x02
#define CONTROLLER_PARAM_BT_ADDR        0x03
#define CONTROLLER_PARAM_BLE_ADDR       0x04

static const uint8_t app_boot_conn_req[] = {'f','r','e','q','c','h','i','p'};//from embedded to pc, request
static const uint8_t app_boot_conn_ack[] = {'F','R','1','0','1','0','O','K'};//from pc to embedded,ack
static const uint8_t app_boot_conn_success[] = {'o','k'};

static const uint8_t controller_param_header[] = {'f', 'r', 'e', 'q'};
static const uint8_t controller_param_tail[] = {'c', 'h', 'i', 'p'};

///default feature
//static uint8_t bt_feature_param[] = {
//    0x41,0x08,
//    0xaf,0x2a,0x4d,0xde,0xc3,0x2f,0x5b,0x87,
//};
static uint8_t btdm_internal_param[] = {
    ///bt_feature:disable 3M(Byte3,bit3),0xde->0xda
    0x41,0x08,
    0xaf,0x2a,0x4d,0xda,0xc3,0x2f,0x5b,0x87,
    ///to add
    
};
/* hardware handlers */
static UART_HandleTypeDef HCI_handle;

/************************************************************************************
 * @fn      controller_start
 *
 * @brief   Initializes bluetooth controller.
 *
 * @param   baudrate: uart baudrate of HCI
 *          ble_addr: ble public address
 *          bt_addr: bt address
 */
bool controller_start(uint32_t baudrate, const uint8_t *ble_addr, const uint8_t *bt_addr)
{
    GPIO_InitTypeDef gpio_config;
    uint8_t buffer[8];
    uint32_t length;
    uint8_t *src, *dst;
    uint16_t tx_length;
    uint8_t opcode, param_type;

    /* configure PA0, PA1, PA2, PA3 to UART0 function */
    gpio_config.Pin = GPIO_PIN_0 | GPIO_PIN_1 | GPIO_PIN_2 | GPIO_PIN_3;
    gpio_config.Mode = GPIO_MODE_AF_PP;
    gpio_config.Pull = GPIO_PULLUP;
    gpio_config.Alternate = GPIO_FUNCTION_1;
    gpio_init(GPIOA, &gpio_config);
    
    /* UART0: used for Log and AT command */
    __SYSTEM_UART0_CLK_ENABLE();
    HCI_handle.UARTx = HCI_UART;
    HCI_handle.Init.BaudRate   = 115200;
    HCI_handle.Init.DataLength = UART_DATA_LENGTH_8BIT;
    HCI_handle.Init.StopBits   = UART_STOPBITS_1;
    HCI_handle.Init.Parity     = UART_PARITY_NONE;
    HCI_handle.Init.FIFO_Mode  = UART_FIFO_ENABLE;
    HCI_handle.TxCpltCallback  = NULL;
    HCI_handle.RxCpltCallback  = NULL;
    uart_init(&HCI_handle);
    /* keep RTS is inactive before HCI is ready */
    __UART_AUTO_FLOW_CONTROL_DISABLE(HCI_handle.UARTx);
    __UART_RTS_INACTIVE(HCI_handle.UARTx);
    
    /* reset controller */
    /* configure PA15 GPIO function */
    __SYSTEM_GPIOA_CLK_ENABLE();
    gpio_config.Pin = GPIO_PIN_15;
    gpio_config.Mode = GPIO_MODE_OUTPUT_PP;
    gpio_config.Pull = GPIO_PULLUP;
    gpio_config.Alternate = GPIO_FUNCTION_0;
    gpio_init(GPIOA, &gpio_config);
    gpio_write_pin(GPIOA, GPIO_PIN_15, 0);
    system_delay_us(3000);
    gpio_write_pin(GPIOA, GPIO_PIN_15, 1);
    
    /* change PA15 to input mode, used to avoid current leakage */
    gpio_config.Pin = GPIO_PIN_15;
    gpio_config.Mode = GPIO_MODE_INPUT;
    gpio_config.Pull = GPIO_NOPULL;
    gpio_config.Alternate = GPIO_FUNCTION_0;
    gpio_init(GPIOA, &gpio_config);
    
    /* hand shake with controller */
    uart_receive(&HCI_handle, buffer, 8);
    while (memcmp(buffer, app_boot_conn_req, 8)) {
        memcpy(&buffer[0], &buffer[1], 7);
        uart_receive(&HCI_handle, &buffer[7], 1);
    }

    uart_transmit(&HCI_handle, (void *)app_boot_conn_ack, 8);
    uart_receive(&HCI_handle, buffer, 2);
    if (memcmp(buffer, app_boot_conn_success, 2)) {
        return false;
    }
    
    /* SWD Enable RAM */
    SWD_W_SystemReg();
    
    /* change uart baudrate */
    opcode = 0x12;
    buffer[0] = 11;   // 921600
    buffer[1] = 0;
    buffer[2] = 0;
    buffer[3] = 0;
    buffer[4] = 0;
    buffer[5] = 0;
    uart_transmit(&HCI_handle, (void *)&opcode, 1);
    uart_transmit(&HCI_handle, (void *)&buffer, 6);
    uart_receive(&HCI_handle, buffer, 7);
    if (buffer[0] != 0x13) {
        return false;
    }
    HCI_handle.Init.BaudRate   = 921600;
    uart_config_baudRate(&HCI_handle);
    system_delay_us(5000);
    
    /* write code into RAM */
#ifndef CONTROLLER_CODE_SPLIT
    opcode = 0x04;
    src = (void *)&CODE_BASE;
    dst = (void *)CODE_EXEC_BASE_ADDR;
    length = (uint32_t)&CODE_END - (uint32_t)&CODE_BASE;
    while (length) {
        tx_length = length > 256 ? 256 : length;
        uart_transmit(&HCI_handle, (void *)&opcode, 1);
        uart_transmit(&HCI_handle, (void *)&dst, 4);
        uart_transmit(&HCI_handle, (void *)&tx_length, 2);
        uart_transmit(&HCI_handle, src, tx_length);
        
        uart_receive(&HCI_handle, buffer, 7);
        if (buffer[0] != 0x05) {
            return false;
        }
        
        length -= tx_length;
        src += tx_length;
        dst += tx_length;
    }
#else
    opcode = 0x04;
    src = (void *)&CODE_L_BASE;
    dst = (void *)CODE_EXEC_L_BASE_ADDR;
    length = (uint32_t)&CODE_L_END - (uint32_t)&CODE_L_BASE;
    while (length) {
        tx_length = length > 256 ? 256 : length;
        uart_transmit(&HCI_handle, (void *)&opcode, 1);
        uart_transmit(&HCI_handle, (void *)&dst, 4);
        uart_transmit(&HCI_handle, (void *)&tx_length, 2);
        uart_transmit(&HCI_handle, src, tx_length);
        
        uart_receive(&HCI_handle, buffer, 7);
        if (buffer[0] != 0x05) {
            return false;
        }
        
        length -= tx_length;
        src += tx_length;
        dst += tx_length;
    }
    
//    opcode = 0x04;
//    src = (void *)&CODE_H_BASE;
//    dst = (void *)CODE_EXEC_H_BASE_ADDR_P2;
//    length = (uint32_t)&CODE_H_END - (uint32_t)&CODE_H_BASE;
//    if (length > (CODE_EXEC_H_BASE_ADDR_P2 - CODE_EXEC_H_BASE_ADDR_P1)) {
//        length -= (CODE_EXEC_H_BASE_ADDR_P2 - CODE_EXEC_H_BASE_ADDR_P1);
//        src += (CODE_EXEC_H_BASE_ADDR_P2 - CODE_EXEC_H_BASE_ADDR_P1);
//    }
//    else {
//        length = 0;
//    }
//    while (length) {
//        tx_length = length > 256 ? 256 : length;
//        uart_transmit(&HCI_handle, (void *)&opcode, 1);
//        uart_transmit(&HCI_handle, (void *)&dst, 4);
//        uart_transmit(&HCI_handle, (void *)&tx_length, 2);
//        uart_transmit(&HCI_handle, src, tx_length);
//        
//        uart_receive(&HCI_handle, buffer, 7);
//        if (buffer[0] != 0x05) {
//            return false;
//        }
//        
//        length -= tx_length;
//        src += tx_length;
//        dst += tx_length;
//    }
#endif

    /* write parameters to exchange memory */
    opcode = 0x04;
    dst = (void *)0x40014000;
    /* write header to remote device */
    uart_transmit(&HCI_handle, (void *)&opcode, 1);
    uart_transmit(&HCI_handle, (void *)&dst, 4);
    length = sizeof(controller_param_header);
    dst += length;
    uart_transmit(&HCI_handle, (void *)&length, 2);
    uart_transmit(&HCI_handle, (void *)&controller_param_header[0], length);
    uart_receive(&HCI_handle, buffer, 7);
    if (buffer[0] != 0x05) {
        return false;
    }
    /* write baudrate to remote device */
    uart_transmit(&HCI_handle, (void *)&opcode, 1);
    uart_transmit(&HCI_handle, (void *)&dst, 4);
    length = sizeof(baudrate) + 1 + 2;
    dst += length;
    uart_transmit(&HCI_handle, (void *)&length, 2);
    param_type = CONTROLLER_PARAM_BAUDRATE;
    uart_transmit(&HCI_handle, (void *)&param_type, 1);
    length = sizeof(baudrate);
    uart_transmit(&HCI_handle, (void *)&length, 2);
    uart_transmit(&HCI_handle, (void *)&baudrate, length);
    uart_receive(&HCI_handle, buffer, 7);
    if (buffer[0] != 0x05) {
        return false;
    }
    /* write keys to remote device */
    length = flashdb_get_length(FDB_KEY_CONTROLLER_INFO) + sizeof(btdm_internal_param);
    if (length) {
        uint8_t *tmp = pvPortMalloc(length);
        uint16_t sub_length = length;
        uint16_t key_len = length - sizeof(btdm_internal_param);
        if(key_len){
            flashdb_get(FDB_KEY_CONTROLLER_INFO, tmp, key_len);
            memcpy(&tmp[key_len],btdm_internal_param,sizeof(btdm_internal_param));
        }
        else{
            memcpy(&tmp[0],btdm_internal_param,sizeof(btdm_internal_param));
        }
        uart_transmit(&HCI_handle, (void *)&opcode, 1);
        uart_transmit(&HCI_handle, (void *)&dst, 4);
        
        length = sub_length + 1 + 2;
        dst += length;
        uart_transmit(&HCI_handle, (void *)&length, 2);
        param_type = CONTROLLER_PARAM_KEY;
        uart_transmit(&HCI_handle, (void *)&param_type, 1);
        length = sub_length;
        uart_transmit(&HCI_handle, (void *)&length, 2);
        uart_transmit(&HCI_handle, (void *)&tmp[0], length);
        uart_receive(&HCI_handle, buffer, 7);
        if (buffer[0] != 0x05) {
            vPortFree(tmp);
            return false;
        }
        vPortFree(tmp);
    }
    /* write bt address to remote device */
    uart_transmit(&HCI_handle, (void *)&opcode, 1);
    uart_transmit(&HCI_handle, (void *)&dst, 4);
    length = 6 + 1 + 2;
    dst += length;
    uart_transmit(&HCI_handle, (void *)&length, 2);
    param_type = CONTROLLER_PARAM_BT_ADDR;
    uart_transmit(&HCI_handle, (void *)&param_type, 1);
    length = 6;
    uart_transmit(&HCI_handle, (void *)&length, 2);
    uart_transmit(&HCI_handle, (void *)&bt_addr[0], length);
    uart_receive(&HCI_handle, buffer, 7);
    if (buffer[0] != 0x05) {
        return false;
    }
    /* write parameter to remote device */
    uart_transmit(&HCI_handle, (void *)&opcode, 1);
    uart_transmit(&HCI_handle, (void *)&dst, 4);
    length = 6 + 1 + 2;
    dst += length;
    uart_transmit(&HCI_handle, (void *)&length, 2);
    param_type = CONTROLLER_PARAM_BLE_ADDR;
    uart_transmit(&HCI_handle, (void *)&param_type, 1);
    length = 6;
    uart_transmit(&HCI_handle, (void *)&length, 2);
    uart_transmit(&HCI_handle, (void *)&ble_addr[0], length);
    uart_receive(&HCI_handle, buffer, 7);
    if (buffer[0] != 0x05) {
        return false;
    }
    /* write tail to remote device */
    uart_transmit(&HCI_handle, (void *)&opcode, 1);
    uart_transmit(&HCI_handle, (void *)&dst, 4);
    length = sizeof(controller_param_tail);
    dst += length;
    uart_transmit(&HCI_handle, (void *)&length, 2);
    uart_transmit(&HCI_handle, (void *)&controller_param_tail[0], length);
    uart_receive(&HCI_handle, buffer, 7);
    if (buffer[0] != 0x05) {
        return false;
    }

    /* boot from RAM */
    opcode = 0x2b;
#ifndef CONTROLLER_CODE_SPLIT
    dst = (void *)CODE_EXEC_BASE_ADDR;
#else
    dst = (void *)CODE_EXEC_L_BASE_ADDR;
#endif
    tx_length = 0;
    uart_transmit(&HCI_handle, (void *)&opcode, 1);
    uart_transmit(&HCI_handle, (void *)&dst, 4);
    uart_transmit(&HCI_handle, (void *)&tx_length, 2);
    uart_receive(&HCI_handle, buffer, 7);
    if (buffer[0] != 0x2c) {
        return false;
    }

#ifdef CONTROLLER_CODE_SPLIT
    system_delay_us(50000);
    opcode = 0x04;
    src = (void *)&CODE_H_BASE;
    dst = (void *)CODE_EXEC_H_BASE_ADDR_P1;
    length = (uint32_t)&CODE_H_END - (uint32_t)&CODE_H_BASE;
//    if (length > (CODE_EXEC_H_BASE_ADDR_P2 - CODE_EXEC_H_BASE_ADDR_P1)) {
//        length = (CODE_EXEC_H_BASE_ADDR_P2 - CODE_EXEC_H_BASE_ADDR_P1);
//    }
    while (length) {
        tx_length = length > 256 ? 256 : length;
        uart_transmit(&HCI_handle, (void *)&opcode, 1);
        uart_transmit(&HCI_handle, (void *)&dst, 4);
        uart_transmit(&HCI_handle, (void *)&tx_length, 2);
        uart_transmit(&HCI_handle, src, tx_length);
        
        uart_receive(&HCI_handle, buffer, 7);
        if (buffer[0] != 0x05) {
            return false;
        }
        
        length -= tx_length;
        src += tx_length;
        dst += tx_length;
    }
    
    /* disconnect */
    opcode = 0x10;
    dst = (void *)1;    /* normal disconnect */
    tx_length = 0;
    uart_transmit(&HCI_handle, (void *)&opcode, 1);
    uart_transmit(&HCI_handle, (void *)&dst, 4);
    uart_transmit(&HCI_handle, (void *)&tx_length, 2);
    uart_receive(&HCI_handle, buffer, 7);
    if (buffer[0] != 0x11) {
        return false;
    }
#endif

    return true;
}