#include <stdint.h>
#include <stdbool.h>
#include <termios.h>
#include <fcntl.h>
#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

/**** baud rates ****/
#define USERIAL_BAUD_300        0
#define USERIAL_BAUD_600        1
#define USERIAL_BAUD_1200       2
#define USERIAL_BAUD_2400       3
#define USERIAL_BAUD_9600       4
#define USERIAL_BAUD_19200      5
#define USERIAL_BAUD_57600      6
#define USERIAL_BAUD_115200     7
#define USERIAL_BAUD_230400     8
#define USERIAL_BAUD_460800     9
#define USERIAL_BAUD_921600     10
#define USERIAL_BAUD_1M         11
#define USERIAL_BAUD_1_5M       12
#define USERIAL_BAUD_2M         13
#define USERIAL_BAUD_3M         14
#define USERIAL_BAUD_4M         15
#define USERIAL_BAUD_AUTO       16

/* Stop Bits */
#define USERIAL_STOPBITS_1      1
#define USERIAL_STOPBITS_1_5    (1<<1)
#define USERIAL_STOPBITS_2      (1<<2)

/* Parity Bits */
#define USERIAL_PARITY_NONE     (1<<3)
#define USERIAL_PARITY_EVEN     (1<<4)
#define USERIAL_PARITY_ODD      (1<<5)

/* Data Bits */
#define USERIAL_DATABITS_5      (1<<6)
#define USERIAL_DATABITS_6      (1<<7)
#define USERIAL_DATABITS_7      (1<<8)
#define USERIAL_DATABITS_8      (1<<9)

#define H4_TYPE_COMMAND         1

#define MSG_STACK_TO_HC_HCI_CMD	0x2000

#define VND_PORT_NAME_MAXLEN    256
#define BT_HS_UART_DEVICE "/dev/ttyHS0"


#define FW_PATCHFILE_PATH	"/system/etc/firmware/BCM4354A2_001.003.015.0064.0175.hcd"

#define HCI_CMD_MAX_LEN             258

/* Common HCI commands */
#define HCI_RESET                               0x0C03
#define HCI_READ_LOCAL_NAME                     0x0C14
#define HCI_READ_LOCAL_BDADDR                   0x1009

/* Vendor Specific HCI commands */
#define HCI_VSC_WRITE_UART_CLOCK_SETTING        0xFC45
#define HCI_VSC_UPDATE_BAUDRATE                 0xFC18
#define HCI_VSC_DOWNLOAD_MINIDRV                0xFC2E
#define HCI_VSC_WRITE_BD_ADDR                   0xFC01
#define HCI_VSC_WRITE_SLEEP_MODE                0xFC27
#define HCI_VSC_WRITE_SCO_PCM_INT_PARAM         0xFC1C
#define HCI_VSC_WRITE_PCM_DATA_FORMAT_PARAM     0xFC1E
#define HCI_VSC_WRITE_I2SPCM_INTERFACE_PARAM    0xFC6D
#define HCI_VSC_ENABLE_WBS                      0xFC7E
#define HCI_VSC_LAUNCH_RAM                      0xFC4E

#define HCI_EVT_CMD_CMPL_STATUS_RET_BYTE        5
#define HCI_EVT_CMD_CMPL_LOCAL_NAME_STRING      6
#define HCI_EVT_CMD_CMPL_LOCAL_BDADDR_ARRAY     6
#define HCI_EVT_CMD_CMPL_OPCODE                 3
#define LPM_CMD_PARAM_SIZE                      12
#define UPDATE_BAUDRATE_CMD_PARAM_SIZE          6
#define HCI_CMD_PREAMBLE_SIZE                   3
#define HCD_REC_PAYLOAD_LEN_BYTE                2
#define BD_ADDR_LEN                             6
#define LOCAL_NAME_BUFFER_LEN                   32
#define LOCAL_BDADDR_PATH_BUFFER_LEN            256

#define STREAM_TO_UINT16(u16, p) {u16 = ((uint16_t)(*(p)) + (((uint16_t)(*((p) + 1))) << 8)); \
		(p) += 2;}
#define UINT8_TO_STREAM(p, u8)   {*(p)++ = (uint8_t)(u8);}
#define UINT16_TO_STREAM(p, u16) {*(p)++ = (uint8_t)(u16); *(p)++ = (uint8_t)((u16) >> 8);}
#define UINT32_TO_STREAM(p, u32) {*(p)++ = (uint8_t)(u32); *(p)++ = (uint8_t)((u32) >> 8); \
		*(p)++ = (uint8_t)((u32) >> 16); *(p)++ = (uint8_t)((u32) >> 24);}


typedef struct
{
    uint16_t          event;
    uint16_t          len;
    uint16_t          offset;
    uint16_t          layer_specific;
    uint8_t           data[];
} HC_BT_HDR;

/* Hardware Configuration State */
enum {
    HW_CFG_START = 1,
    HW_CFG_SET_UART_CLOCK,
    HW_CFG_SET_UART_BAUD_1,
    HW_CFG_READ_LOCAL_NAME,
    HW_CFG_DL_MINIDRIVER,
    HW_CFG_DL_FW_PATCH,
    HW_CFG_SET_UART_BAUD_2,
    HW_CFG_SET_BD_ADDR
#if (USE_CONTROLLER_BDADDR == TRUE)
    , HW_CFG_READ_BD_ADDR
#endif
};

/* Structure used to configure serial port during open */
typedef struct
{
    uint16_t fmt;       /* Data format */
    uint8_t  baud;      /* Baud rate */
} tUSERIAL_CFG;


typedef struct
{
    int fd;                     /* fd to Bluetooth device */
    struct termios termios;     /* serial terminal of BT port */
    char port_name[VND_PORT_NAME_MAXLEN];
} vnd_userial_cb_t;


vnd_userial_cb_t vnd_userial;

static const tUSERIAL_CFG userial_init_cfg =
{
    (USERIAL_DATABITS_8 | USERIAL_PARITY_NONE | USERIAL_STOPBITS_1),
    USERIAL_BAUD_115200
};


void userial_vendor_init(void)
{
	vnd_userial.fd = -1;
	snprintf(vnd_userial.port_name, VND_PORT_NAME_MAXLEN, "%s",
			BT_HS_UART_DEVICE);
}


void userial_to_tcio_baud(uint8_t cfg_baud, uint32_t *baud)
{
    if (cfg_baud == USERIAL_BAUD_115200)
        *baud = B115200;
    else if (cfg_baud == USERIAL_BAUD_4M)
        *baud = B4000000;
    else if (cfg_baud == USERIAL_BAUD_3M)
        *baud = B3000000;
    else if (cfg_baud == USERIAL_BAUD_2M)
        *baud = B2000000;
    else if (cfg_baud == USERIAL_BAUD_1M)
        *baud = B1000000;
    else if (cfg_baud == USERIAL_BAUD_921600)
        *baud = B921600;
    else if (cfg_baud == USERIAL_BAUD_460800)
        *baud = B460800;
    else if (cfg_baud == USERIAL_BAUD_230400)
        *baud = B230400;
    else if (cfg_baud == USERIAL_BAUD_57600)
        *baud = B57600;
    else if (cfg_baud == USERIAL_BAUD_19200)
        *baud = B19200;
    else if (cfg_baud == USERIAL_BAUD_9600)
        *baud = B9600;
    else if (cfg_baud == USERIAL_BAUD_1200)
        *baud = B1200;
    else if (cfg_baud == USERIAL_BAUD_600)
        *baud = B600;
    else
        *baud = B115200;
}


int userial_vendor_open(tUSERIAL_CFG *p_cfg)
{
    uint32_t baud;
    uint8_t data_bits;
    uint16_t parity;
    uint8_t stop_bits;

	userial_to_tcio_baud(p_cfg->baud, &baud);

    if(p_cfg->fmt & USERIAL_DATABITS_8)
        data_bits = CS8;
    else if(p_cfg->fmt & USERIAL_DATABITS_7)
        data_bits = CS7;
    else if(p_cfg->fmt & USERIAL_DATABITS_6)
        data_bits = CS6;
    else if(p_cfg->fmt & USERIAL_DATABITS_5)
        data_bits = CS5;
    else
        return -1;

    if(p_cfg->fmt & USERIAL_PARITY_NONE)
        parity = 0;
    else if(p_cfg->fmt & USERIAL_PARITY_EVEN)
        parity = PARENB;
    else if(p_cfg->fmt & USERIAL_PARITY_ODD)
        parity = (PARENB | PARODD);
    else
        return -1;

    if(p_cfg->fmt & USERIAL_STOPBITS_1)
        stop_bits = 0;
    else if(p_cfg->fmt & USERIAL_STOPBITS_2)
        stop_bits = CSTOPB;
    else
        return -1;

    printf("userial vendor open: opening %s\n", vnd_userial.port_name);

    if ((vnd_userial.fd = open(vnd_userial.port_name, O_RDWR)) == -1) {
        printf("userial vendor open: unable to open %s\n",
			vnd_userial.port_name);
        return -1;
    }

    tcflush(vnd_userial.fd, TCIOFLUSH);

    tcgetattr(vnd_userial.fd, &vnd_userial.termios);
    cfmakeraw(&vnd_userial.termios);
    vnd_userial.termios.c_cflag |= (CRTSCTS | stop_bits);
    tcsetattr(vnd_userial.fd, TCSANOW, &vnd_userial.termios);
    tcflush(vnd_userial.fd, TCIOFLUSH);

    tcsetattr(vnd_userial.fd, TCSANOW, &vnd_userial.termios);
    tcflush(vnd_userial.fd, TCIOFLUSH);
    tcflush(vnd_userial.fd, TCIOFLUSH);

    /* set input/output baudrate */
    cfsetospeed(&vnd_userial.termios, baud);
    cfsetispeed(&vnd_userial.termios, baud);
    tcsetattr(vnd_userial.fd, TCSANOW, &vnd_userial.termios);

    printf("device fd = %d open\n", vnd_userial.fd);

    return vnd_userial.fd;
}

void userial_vendor_set_baud(uint8_t userial_baud)
{
    uint32_t tcio_baud;

    userial_to_tcio_baud(userial_baud, &tcio_baud);

    cfsetospeed(&vnd_userial.termios, tcio_baud);
    cfsetispeed(&vnd_userial.termios, tcio_baud);
    tcsetattr(vnd_userial.fd, TCSANOW, &vnd_userial.termios);
}

uint8_t userial_write(const uint8_t *p_data, uint8_t len)
{
	uint8_t total = 0;
	uint8_t type = H4_TYPE_COMMAND;

	ssize_t ret = write(vnd_userial.fd, &type, 1);
	if (ret < 1) {
                printf("%s error writing to serial port: %s\n", __func__,
				strerror(errno));
		return 0;
	}

	while (len) {
		ret = write(vnd_userial.fd, p_data + total, len);
		switch (ret) {
		case -1:
			printf("%s error writing to serial port: %s\n",
					__func__, strerror(errno));
			return total;
		case 0:  // don't loop forever in case write returns 0.
			return total;
		default:
			total += ret;
			len -= ret;
		break;
		}
	}

	return total;
}

uint8_t vnd_local_bd_addr[6]={0x90, 0xE7, 0xC4, 0xF3, 0xF7, 0x09};

void ms_delay (uint32_t timeout)
{
    struct timespec delay;
    int err;

    if (timeout == 0)
        return;

    delay.tv_sec = timeout / 1000;
    delay.tv_nsec = 1000 * 1000 * (timeout%1000);

    /* [u]sleep can't be used because it uses SIGALRM */
    do {
        err = nanosleep(&delay, &delay);
    } while (err < 0 && errno ==EINTR);
}


int main(int argc, char *argv[])
{
	HC_BT_HDR *p_buf = NULL;
	uint8_t *p;
	uint16_t opcode;
	int fd, fw_fd;
	int ret = -1;
	int delay = 100;

	userial_vendor_init();

	fd = userial_vendor_open((tUSERIAL_CFG *)&userial_init_cfg);
	if (fd < 0) {
		printf("Failed to open firmware patch file\n");
		return -1;
	}

	p_buf = malloc(sizeof(*p_buf) + HCI_CMD_MAX_LEN);
	if (!p_buf) {
		printf("Failed to malloc p_buf\n");
		goto alloc_failed;
	}


        p_buf->event = MSG_STACK_TO_HC_HCI_CMD;
        p_buf->offset = 0;
        p_buf->layer_specific = 0;

	// HCI_RESET
        p = (uint8_t *) (p_buf + 1);
        UINT16_TO_STREAM(p, HCI_RESET);
        *p = 0; /* parameter length */
        p_buf->len = HCI_CMD_PREAMBLE_SIZE;
        userial_write(p_buf->data, p_buf->len);


	// HCI_VSC_WRITE_UART_CLOCK_SETTING - 48M for 4M baudrate
        p = (uint8_t *) (p_buf + 1);
        UINT16_TO_STREAM(p, HCI_VSC_WRITE_UART_CLOCK_SETTING);
	*p++ = 1; /* parameter length */
	*p = 1; /* (1,"UART CLOCK 48 MHz")(2,"UART CLOCK 24 MHz") */

	p_buf->len = HCI_CMD_PREAMBLE_SIZE + 1;
        userial_write(p_buf->data, p_buf->len);

	// HCI_VSC_UPDATE_BAUDRATE	- 4M
        p = (uint8_t *) (p_buf + 1);
	UINT16_TO_STREAM(p, HCI_VSC_UPDATE_BAUDRATE);
	*p++ = UPDATE_BAUDRATE_CMD_PARAM_SIZE; /* parameter length */
	*p++ = 0; /* encoded baud rate */
	*p++ = 0; /* use encoded form */

	UINT32_TO_STREAM(p, 4000000);

	p_buf->len = HCI_CMD_PREAMBLE_SIZE + UPDATE_BAUDRATE_CMD_PARAM_SIZE;
        userial_write(p_buf->data, p_buf->len);

	printf("bt vendor lib: set UART baud 4000000\n");
	userial_vendor_set_baud(USERIAL_BAUD_4M);

	// SKIP HCI_READ_LOCAL_NAME

	fw_fd = open(FW_PATCHFILE_PATH, O_RDONLY);
	if (fw_fd < 0) {
		printf("Failed to open %s\n", FW_PATCHFILE_PATH);
		goto open_failed;
	}

	// HCI_VSC_DOWNLOAD_MINIDRV - download entire patchfile in this state
        p = (uint8_t *) (p_buf + 1);
	UINT16_TO_STREAM(p, HCI_VSC_DOWNLOAD_MINIDRV);
	*p = 0; /* parameter length */
	p_buf->len = HCI_CMD_PREAMBLE_SIZE;

        userial_write(p_buf->data, p_buf->len);
	ms_delay(50);

	int j = 0;

read_again:
        p = (uint8_t *) (p_buf + 1);
	p_buf->len = read(fw_fd, p, HCI_CMD_PREAMBLE_SIZE);
	opcode = (*(p + 1) << 8) | *p;	
	if (p_buf->len == HCI_CMD_PREAMBLE_SIZE) {
		p_buf->len += read(fw_fd, p+HCI_CMD_PREAMBLE_SIZE,
				*(p+HCD_REC_PAYLOAD_LEN_BYTE));
/*

		printf("HCI command[%i] - opcode: %x, len: %d\n", j++,
				opcode, p_buf->len);
		for (int i = 0; i < p_buf->len; i++) {
			printf("%x ", p_buf->data[i]);
		}
		printf("\n");
*/

		if (opcode != HCI_VSC_LAUNCH_RAM)
			userial_write(p_buf->data, p_buf->len);
		else
			printf("HCI_VSC_LAUNCH_RAM received!\n");

		goto read_again;
	}

	printf("bt vendor lib: set UART baud 115200\n");
	userial_vendor_set_baud(USERIAL_BAUD_115200);

	ms_delay(100);

	// HCI_RESET
        p = (uint8_t *) (p_buf + 1);
        UINT16_TO_STREAM(p, HCI_RESET);
        *p = 0; /* parameter length */
        p_buf->len = HCI_CMD_PREAMBLE_SIZE;
        userial_write(p_buf->data, p_buf->len);

	printf("bt vendor lib: set UART baud 4000000\n");
	userial_vendor_set_baud(USERIAL_BAUD_4M);

	// HW_VSC_WRITE_BD_ADDR

	printf("Setting local bd addr to %02X:%02X:%02X:%02X:%02X:%02X\n",
		vnd_local_bd_addr[0], vnd_local_bd_addr[1],
		vnd_local_bd_addr[2], vnd_local_bd_addr[3],
		vnd_local_bd_addr[4], vnd_local_bd_addr[5]);

        p = (uint8_t *) (p_buf + 1);
	UINT16_TO_STREAM(p, HCI_VSC_WRITE_BD_ADDR);
	*p++ = BD_ADDR_LEN; /* parameter length */
	*p++ = vnd_local_bd_addr[5];
	*p++ = vnd_local_bd_addr[4];
	*p++ = vnd_local_bd_addr[3];
	*p++ = vnd_local_bd_addr[2];
	*p++ = vnd_local_bd_addr[1];
	*p = vnd_local_bd_addr[0];

	p_buf->len = HCI_CMD_PREAMBLE_SIZE + BD_ADDR_LEN;
        userial_write(p_buf->data, p_buf->len);

	printf("vendor lib fwcfg completed\n");
	close(fw_fd);

	ret = 0;

open_failed:

	free(p_buf);

alloc_failed:
	close(fd);

	return ret;

}