File content as of revision 3:35b05d91568d:

#include "mbed.h"
#include "cmsis_os.h"
#include "max32630fthr.h"
#include "USBSerial.h"
#include "CmdHandler.h"
#include "DUT_RegConfig.h"
#include "ServoRun.h"
#include "SDFileSystem.h"
#include "Firmware.h"
#include "RegTable.h"
#include "CmdHandler.h"

DUTREG dut_reg[DUT_REG_NUM];
uint8_t Firmware[8192];
uint8_t histogram[10][1024];
uint8_t dcr_matrix[17][9*2];

uint8_t int_mark = 0;
uint8_t int_enable = 1;
uint8_t histogram_mode = 0;
uint8_t histogram_tdc = 0;

Semaphore chip_int_semph(1);
extern DigitalOut xSHUT;
extern I2C i2c_v;
extern DigitalOut TRIM_EN;

uint16_t histogram_pos_num = 0;
uint16_t histogram_per_pos = 0;

uint16_t range_step_num = 0;
uint16_t range_per_step = 0;

extern const uint8_t reg_table[];
extern const uint8_t Firmware_Ranging[];
extern uint8_t  _uart_send_pbuff[CMD_BUF_LEN] ;

void ChipInitReset(void)
{
    xSHUT = 0;
    wait_ms(30);
    xSHUT = 1;
    wait_ms(30);
}

void SetBitThree(uint8_t rco, uint8_t tdc, uint8_t dcr)
{
    WriteOneReg(0x37, rco);
    WriteOneReg(0xE4, tdc);
    WriteOneReg(0xC0, dcr);
}

void DUT_RegInit(uint8_t rco, uint8_t tdc, uint8_t dcr)
{
    LoadRegTable();
    for(uint16_t i = 0; i < 256; i++) 
    {
        WriteOneReg(dut_reg[i].addr, dut_reg[i].value);        
    }

    SetBitThree(rco, tdc, dcr);
}

void DUT_FirmwareInit(void)
{
    WriteFW(LoadFirmware());
}

void DeviceAllInit(uint8_t rco, uint8_t tdc, uint8_t dcr)
{
    ChipInitReset();
    wait_ms(100);
    DUT_RegInit(rco, tdc, dcr);
    wait_ms(100);
    DUT_FirmwareInit();
    wait_ms(100);
}

void Enable_DUT_Interrupt(void)
{
    int_enable = 1;
}

void Disable_DUT_Interrupt(void)
{
    int_enable = 0;
}

void InterruptHandle(void)
{
    if(int_enable == 0)
        return;

    if(int_mark == 1) {
        int_mark = 0;
    } else if(int_mark == 2) {
        chip_int_semph.release();
        //发送信号量
    }
}



void HistogramReport()
{
    uint8_t ret = 0;
    uint16_t lsb, mili, noise_level;
    uint32_t peak;


    uint16_t histogram_pos = 0;
    uint16_t histogram_num = 0;
    uint16_t range_step = 0;
    while(1) {
        if(histogram_mode == 1) {
            //ReadOneReg(REG_SYS_CFG, &sys_cfg_save);
            //WriteOneReg(REG_SYS_CFG, 0x00);
            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level);
            if(ret != 0) {
                histogram_mode = 0;
            } else {
                ret = vangogh_ram_rd(histogram_tdc);
                if(ret != 0) {
                    histogram_mode = 0;
                } else {
                    HandleReadHistogram(histogram_tdc);
                }
            }
            //WriteOneReg(REG_SYS_CFG, sys_cfg_save);
        } else if(histogram_mode == 2) {
            //ReadOneReg(REG_SYS_CFG, &sys_cfg_save);
            //WriteOneReg(REG_SYS_CFG, 0x00);
            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level);
            if(ret != 0) {
                histogram_mode = 0;
            } else {
                for(uint8_t i = 0; i < 10; i++) {
                    ret = vangogh_ram_rd(i);
                    if(ret != 0) {
                        histogram_mode = 0;
                        break;
                    } else {
                        HandleReadHistogram(i);
                        wait_ms(1);
                    }
                }
            }
            //WriteOneReg(REG_SYS_CFG, sys_cfg_save);
        } else if(histogram_mode == 3) {


            if(histogram_num >= histogram_per_pos) {
                histogram_num = 0;
                histogram_pos++;
                if(histogram_pos >= histogram_pos_num) {
                    histogram_mode = 0;
                }
                ServoRun(1, 10);
                while(CheckUntil()) wait_ms(100);
            }
            //ReadOneReg(REG_SYS_CFG, &sys_cfg_save);
            //WriteOneReg(REG_SYS_CFG, 0x00);
            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level);
            if(ret != 0) {
                histogram_mode = 0;
            } else {
                for(uint8_t i = 0; i < 10; i++) {
                    ret = vangogh_ram_rd(i);
                    if(ret != 0) {
                        histogram_mode = 0;
                        break;
                    } else {
                        StoreHistogram(histogram_pos, histogram_num, i);
                        wait_ms(100);
                    }
                }
            }
            //WriteOneReg(REG_SYS_CFG, sys_cfg_save);
            histogram_num++;
        } else if(histogram_mode == 4) {
            //ReadOneReg(REG_SYS_CFG, &sys_cfg_save);
            //WriteOneReg(REG_SYS_CFG, 0x00);
            ret = OneTimeMeasure(&lsb, &mili, &peak, &noise_level);
            if(ret != 0) {
                histogram_mode = 0;
            } else {
                for(uint8_t i = 0; i < 9; i++) {
                    ret = vangogh_ram_rd(i);
                    if(ret != 0) {
                        histogram_mode = 0;
                        break;
                    } else {
                        HandleReadHistogram(i);
                        wait_ms(1);
                    }
                }
            }
            //WriteOneReg(REG_SYS_CFG, sys_cfg_save);
        } else if(histogram_mode == 5) {

            if(range_per_step == 0) {
                range_step++;
                if(range_step >= range_step_num) {
                    histogram_mode = 0;
                }
                ServoRun(1, 10);
                while(CheckUntil()) wait_ms(100);

                range_per_step = 0;
                ret = ContinuousMeasure();
                if(ret != 0) {
                    histogram_mode = 0;
                } else {
                    while(range_per_step<256 && histogram_mode == 5) {
                        wait_ms(100);
                    }
                    StoreHistogram(range_step, 0, 0);
                    wait_ms(5);
                    StoreHistogram(range_step, 0, 1);
                    wait_ms(5);
                    StoreHistogram(range_step, 0, 2);
                    wait_ms(5);
                    StoreHistogram(range_step, 0, 3);
                    wait_ms(5);
                    range_per_step = 0;

                }
            }



        } else {
            histogram_num = 0;
            histogram_pos = 0;
            range_step = 0;
            histogram_pos_num = 0;
            histogram_per_pos = 0;
        }
        wait(1);
    }
}

void StoreMeasureData(uint16_t lsb, uint16_t milimeter, uint32_t peak, uint16_t noise_level, uint16_t i)
{
    histogram[0][4*i] = 0;
    histogram[0][4*i+1] = 0;
    histogram[0][4*i+2] = lsb >> 8;
    histogram[0][4*i+3] = lsb ;
    
    histogram[1][4*i] = 0;
    histogram[1][4*i+1] = 0;
    histogram[1][4*i+2] = milimeter >> 8;
    histogram[1][4*i+3] = milimeter ;
    
    histogram[2][4*i]   = peak >> 24;
    histogram[2][4*i+1] = peak >> 16;
    histogram[2][4*i+2] = peak >> 8;
    histogram[2][4*i+3] = peak ;
    
    histogram[3][4*i]   = 0;
    histogram[3][4*i+1] = 0;
    histogram[3][4*i+2] = noise_level >> 8;
    histogram[3][4*i+3] = noise_level ;
    
}

void ContinuousMeasureReport()
{
    uint16_t lsb, milimeter, noise_level;
    uint32_t peak;
    uint8_t int_flag = 0;
    uint16_t time_out = 0;

    while(1) {

        chip_int_semph.wait();
        if(histogram_mode == 5) {
            if(RaadContinuousMeasure(&lsb, &milimeter, &peak, &noise_level) == 0) {
                StoreMeasureData(lsb, milimeter, peak, noise_level, range_per_step++);
                if(range_per_step == 256) {
                    StopContinuousMeasure();
                }
            }
        } else {
            if(RaadContinuousMeasure(&lsb, &milimeter, &peak, &noise_level) == 0) {
                HandleContinuousMeasureReport(lsb, milimeter, peak, noise_level);
            }
        }

    }
}

uint8_t WriteOneReg(uint8_t addr, uint8_t data)
{
    uint8_t buf[2];
    buf[0] = addr;
    buf[1] = data;

    i2c_v.write(DUT_DEV_ADDR, (char*)buf, 2);

    return 0;
}

uint8_t ReadOneReg(uint8_t addr, uint8_t *data)
{
    uint8_t buf[2];
    buf[0] = addr;

    i2c_v.write(DUT_DEV_ADDR, (char*)buf,  1);
    i2c_v.read (DUT_DEV_ADDR, (char*)data, 1);

    return 0;
}


uint8_t ReadAllRegToTable(void)
{
    for(uint16_t i = 0; i < DUT_REG_NUM; i++) {
        ReadOneReg(dut_reg[i].addr, &(dut_reg[i].value));
    }

    return 0;
}

uint8_t WriteFW(uint16_t size)
{
    uint8_t  ret = 0;
    uint8_t  i;
    uint8_t  reg_sys_cfg;
    uint8_t  uart_rx_data;
    uint16_t fw_size = size;
    uint16_t fw_send = 0;

    ret = WriteOneReg(REG_PW_CTRL, 0x08);
    ret = WriteOneReg(REG_PW_CTRL, 0x0a);
    ret = WriteOneReg(REG_MCU_CFG, 0x02);
    ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg);
    ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
    ret = WriteOneReg(REG_CMD, 0x01);
    ret = WriteOneReg(REG_SIZE, 0x02);
    ret = WriteOneReg(REG_SCRATCH_PAD_BASE+0x00, 0x00);
    ret = WriteOneReg(REG_SCRATCH_PAD_BASE+0x01, 0x00);

    while(fw_size >= 32) {
        ret = WriteOneReg(REG_CMD,0x03);
        ret = WriteOneReg(REG_SIZE,0x20);
        for(i=0; i<32; i++) {
            uart_rx_data = Firmware[fw_send++];
            ret = WriteOneReg(REG_SCRATCH_PAD_BASE+i, uart_rx_data);
            wait_us(5);
        }
        fw_size -= 32;
    }
    if(fw_size > 0) {
        ret = WriteOneReg(REG_CMD,0x03);
        ret = WriteOneReg(REG_SIZE,(uint8_t)fw_size);
        for(i=0; i<fw_size; i++) {
            uart_rx_data = Firmware[fw_send++];
            ret = WriteOneReg(REG_SCRATCH_PAD_BASE+i, uart_rx_data);
            wait_us(5);
        }
    }
    ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0));
    ret = WriteOneReg(REG_MCU_CFG, 0x03);
    ret = WriteOneReg(REG_PW_CTRL, 0x02); //reset r_otp_ld_done, is a must
    ret = WriteOneReg(REG_PW_CTRL, 0x00); //reset r_otp_ld_done, exit low power mode

    return ret;
}

uint8_t vangogh_ram_rd(uint8_t tdc)       //UART CMD foramt: CMD-1-byte|TDC-index-1-byte
{
    uint8_t ret = 0;
    uint8_t i;
    uint8_t j;
    uint8_t reg_pw_ctrl;
    uint8_t reg_sys_cfg;
    uint8_t tdc_index = tdc;
    uint16_t ram_addr_base = 0x1000 + 0x0400 * tdc_index;

    ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
    ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
    ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
    ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
    ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3) | (0x01<<1)); //set otp_ld_done, pw_ctrl_lp
    ret = WriteOneReg(REG_MCU_CFG, 0x01);
    ret = WriteOneReg(REG_CMD, 0x01);
    ret = WriteOneReg(REG_SIZE, 0x02);
    ret = WriteOneReg(REG_SCRATCH_PAD_BASE+0x00, *((uint8_t*)(&ram_addr_base)  ));
    ret = WriteOneReg(REG_SCRATCH_PAD_BASE+0x01, *((uint8_t*)(&ram_addr_base)+1));
    wait_us(100);
    for(j=0; j<32; j++) {
        ret = WriteOneReg(REG_CMD,0x05);  //Issue RAM read command
        ret = WriteOneReg(REG_SIZE,0x20); //Issue RAM read command
        wait_us(100);
        for(i=0; i<32; i++) {
            ret = ReadOneReg(0x0c+i, &histogram[tdc_index][32*j + i]);
        }
    }
    ret = WriteOneReg(REG_MCU_CFG, 0x03);
    ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en
    ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<1)); //restore power         control register
    ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl); //clear pw_ctrl_lp

    return ret;
}

uint8_t OneTimeMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level)
{
    uint8_t ret = 0;
    uint8_t reg_pw_ctrl;
    uint8_t reg_sys_cfg;
    uint32_t timeout = 0;
    uint8_t flag = 0;

    int_mark = 1;//One Time Measure

    //ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
    //ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
    //ret = WriteOneReg(REG_PW_CTRL, (reg_pw_ctrl | (0x01<<3)) & ~(0x01<<1)); //set otp_ld_done, clear pw_ctrl_lp
    //ret = WriteOneReg(0x08, 0x00);
    ret = WriteOneReg(REG_CMD, 0x0E);
    //ret = WriteOneReg(REG_SIZE, 0x00);

    timeout = 1000;
    while(int_mark == 1 && timeout != 0) {
        timeout--;
        wait_ms(5);
        //ReadOneReg(0x08, &flag);
        // if(flag == 0xFF)
        //{
        //     int_mark = 0;
        //  }
    }

    if(timeout == 0) {
        return 1;

    } else {

        ret = ReadOneReg(0x0d, (uint8_t*)lsb);
        ret = ReadOneReg(0x0e, (uint8_t*)lsb + 1);

        ret = ReadOneReg(0x18, (uint8_t*)milimeter);
        ret = ReadOneReg(0x19, (uint8_t*)milimeter + 1);

        ret = ReadOneReg(0x26, (uint8_t*)noise_level);
        ret = ReadOneReg(0x27, (uint8_t*)noise_level + 1);

        ret = ReadOneReg(0x28, (uint8_t*)peak );
        ret = ReadOneReg(0x29, (uint8_t*)peak + 1);
        ret = ReadOneReg(0x2a, (uint8_t*)peak + 2);
        ret = ReadOneReg(0x2b, (uint8_t*)peak + 3);

    }

    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en
    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<1)); //restore power         control register
    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl & ~(0x01<<1)); //clear pw_ctrl_lp

    return ret;
}

uint8_t ContinuousMeasure(void)
{
    uint8_t ret = 0;
    uint8_t reg_pw_ctrl;
    uint8_t reg_sys_cfg;

    int_mark = 2;//Continuous Time Measure

    //ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
    //ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
    //ret = WriteOneReg(REG_PW_CTRL, (reg_pw_ctrl | (0x01<<3)) & ~(0x01<<1)); //set otp_ld_done, clear pw_ctrl_lp
    ret = WriteOneReg(REG_CMD, 0x0F);
    //ret = WriteOneReg(REG_SIZE, 0x00);

    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en

    return ret;
}

uint8_t RaadContinuousMeasure(uint16_t *lsb, uint16_t *milimeter, uint32_t *peak, uint16_t *noise_level)
{
    uint8_t ret = 0;
    uint8_t reg_pw_ctrl;
    uint8_t reg_sys_cfg;

    int_mark = 2;//Continuous Time Measure

    //ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
    //ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
    //ret = WriteOneReg(REG_PW_CTRL, (reg_pw_ctrl | (0x01<<3)) & ~(0x01<<1)); //set otp_ld_done, clear pw_ctrl_lp

    ret = ReadOneReg(0x0d, (uint8_t*)lsb);
    ret = ReadOneReg(0x0e, (uint8_t*)lsb + 1);

    ret = ReadOneReg(0x18, (uint8_t*)milimeter);
    ret = ReadOneReg(0x19, (uint8_t*)milimeter + 1);

    ret = ReadOneReg(0x26, (uint8_t*)noise_level);
    ret = ReadOneReg(0x27, (uint8_t*)noise_level + 1);

    ret = ReadOneReg(0x28, (uint8_t*)peak );
    ret = ReadOneReg(0x29, (uint8_t*)peak + 1);
    ret = ReadOneReg(0x2a, (uint8_t*)peak + 2);
    ret = ReadOneReg(0x2b, (uint8_t*)peak + 3);

    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en

    return ret;
}

uint8_t StopContinuousMeasure()
{
    uint8_t ret = 0;
    uint8_t reg_pw_ctrl;
    uint8_t reg_sys_cfg;

    int_mark = 0;//Continuous Time Measure

    //ret = ReadOneReg (REG_SYS_CFG, &reg_sys_cfg           );
    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg | (0x01<<0));
    //ret = ReadOneReg (REG_PW_CTRL, &reg_pw_ctrl);
    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<3)); //set otp_ld_done
    //ret = WriteOneReg(REG_PW_CTRL, (reg_pw_ctrl | (0x01<<3)) & ~(0x01<<1)); //set otp_ld_done, clear pw_ctrl_lp
    ret = WriteOneReg(REG_CMD, 0x00);
    ret = WriteOneReg(REG_SIZE, 0x00);

    //ret = WriteOneReg(REG_SYS_CFG, reg_sys_cfg & ~(0x01<<0)); //clear sc_en
    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl | (0x01<<1)); //restore power         control register
    //ret = WriteOneReg(REG_PW_CTRL, reg_pw_ctrl & ~(0x01<<1)); //clear pw_ctrl_lp

    return ret;
}

void StoreHistogram(uint16_t histogram_pos, uint16_t histogram_num, uint8_t tdc)
{
    /*
    char file_name[20];

    //sprintf(file_name, "/sd/hist_%d_%d_tdc%d", histogram_pos, histogram_num, tdc);
    sprintf(file_name, "/sd/hello.txt");

    FILE *fp = fopen(file_name, "w");
    //for(uint32_t i = 0; i < 1024; i++) {
    //    fseek(fp,0,SEEK_END);
    //    fprintf(fp, "%02X ", histogram[tdc][i]);
    //}
    fprintf(fp, "hello ");

    fflush(fp);
    fclose(fp);
    */

    _uart_send_pbuff[0] = histogram_pos;
    _uart_send_pbuff[1] = histogram_pos >> 8;

    _uart_send_pbuff[2] = tdc;

    _uart_send_pbuff[3] = histogram_num;
    _uart_send_pbuff[4] = histogram_num >> 8;

    memcpy(&_uart_send_pbuff[5], histogram[tdc], 1024);

    UART_CmdAckSend(READ_CMD | 0x80, VAN_STEP_HISTOGRAM_CMD, _uart_send_pbuff, 1024 + 1 + 2 + 2);
    //return 0;
}

uint8_t DCRTest(uint8_t vspad, uint8_t test_time)
{
    uint8_t ret = 0;
    uint32_t timeout = 0;
    memset(dcr_matrix, 0x00, 17*9*2);
    //WriteOneReg(0x50, 0x01);
    //WriteOneReg(0xBD, 0x01);
    //wait_ms(100);
    //WriteOneReg(0xD8, 0xF0);
    WriteOneReg(0xC0, vspad);

    for(uint8_t group = 0; group < 9; group++) {
        WriteOneReg(0x0C, group);
        wait_ms(1);
        WriteOneReg(0x0D, test_time);
        wait_ms(1);

        timeout = 25;
        int_mark = 1;
        WriteOneReg(0x0A, 0x0C);
        while(int_mark == 1 && timeout != 0) {
            timeout--;
            wait_ms(100);
        }
        //osDelay(80);
        for(uint8_t pix = 0; pix < 17; pix++) {
            ret = ReadOneReg(0x82 + pix*2, &dcr_matrix[pix][group*2 + 1]);
            ret = ReadOneReg(0x82 + pix*2 + 1, &dcr_matrix[pix][group*2]);
            //osDelay(1);
            if(ret != 0)
                return ret;
        }

    }

    return 0;
}

uint8_t DelayLineTest(uint8_t phase, uint8_t* buf)
{
    uint8_t ret = 0;
    uint32_t timeout = 0;

    WriteOneReg(0xE4, phase);
    wait_ms(10);

    for(uint8_t step = 0; step < 8; step++) {
        WriteOneReg(0xE1, 0x61 | (step << 1));
        wait_ms(10);

        timeout = 50;
        int_mark = 1;
        WriteOneReg(0x0A, 0x0B);
        wait_ms(10);
        while(int_mark == 1 && timeout != 0) {
            timeout--;
            wait_ms(100);
        }
        for(uint8_t tdc = 0; tdc < 9; tdc++) {
            ret = ReadOneReg(0x0c + tdc*2, buf + step*18 + tdc*2 + 1);
            wait_ms(1);
            ret = ReadOneReg(0x0c + tdc*2 + 1, buf + step*18 + tdc*2);
            wait_ms(1);
        }

    }

    return 0;
}

uint8_t GetTdcPhase(uint8_t* buf)
{
    uint8_t ret = 0;
    uint32_t timeout = 0;
    memset(buf, 0x00, 10);

    WriteOneReg(0x0C, 0x0C);
    WriteOneReg(0x0A, 0x0A);
    wait_ms(1);

    timeout = 10;
    int_mark = 1;
    WriteOneReg(0x0A, 0x0B);
    while(int_mark == 1 && timeout != 0) {
        timeout--;
        wait_ms(100);
    }
    //osDelay(80);

    ret = ReadOneReg(0x0D, buf);
    ret = ReadOneReg(0xE4, buf + 1);

    if(ret != 0)
        return ret;

    return 0;
}

uint8_t SetWindow(uint8_t* pd)
{
    uint8_t ret = 0;
    uint8_t buf[14];
    
    memset(buf, 0x00, 14);
    
    buf[0] |= (pd[5]&0xF);
    buf[0] |= ((pd[6] << 4)&0xF0);
    
    buf[1] |= (pd[7]&0xF);
    buf[1] |= ((pd[8] << 4)&0xF0);
    
    buf[2] |= (pd[9]&0xF);
    buf[2] |= ((pd[10] << 4)&0xF0);
    
    buf[3] |= (pd[11]&0xF);
    buf[3] |= ((pd[12] << 4)&0xF0);
    
    buf[4] |= (pd[13]&0xF);
    buf[4] |= ((pd[14] << 4)&0xF0);
    
    buf[5] |= (pd[15]&0xF);
    buf[5] |= ((pd[16] << 4)&0xF0);
    
    buf[6] |= (pd[17]&0xF);
    buf[6] |= ((pd[18] << 4)&0xF0);
    
    buf[7] |= (pd[19]&0xF);
    buf[7] |= ((pd[20] << 4)&0xF0);
    
    buf[8] = 0x00;// Fisrt 16LSB cut off
    buf[9] = 0xFF;
    
    buf[10] = 0x01;// Ref PAD switch
    
    buf[11] = 0x1F;// TDC Resolution 1F = 31
    
    buf[12] = 0x0C;// Length
    buf[13] = 0x09;//CMD
    
    for(uint8_t i = 0; i < 12; i++)
    {
        WriteOneReg(0x0C + i, buf[i]);
    }
    
    WriteOneReg(0x0B, 0x0C);
    WriteOneReg(0x0A, 0x09);
    
    return 0;
}

uint8_t RCO_Trim(uint8_t *rco)
{
    uint8_t ret = 0;
    uint8_t value = 0, result = 0;
    
    TRIM_EN = 1;

    //==================First BAND=====================//
    ChipInitReset();
    ret = WriteOneReg(0xED    , 0x03);
    ret = WriteOneReg(0xEC    , 0x36);
    ret = WriteOneReg(0x3D    , 0xC0);
    ret = WriteOneReg(0xE8    , 0x00);
    ret = WriteOneReg(0xEA    , 0x3F);
    ret = WriteOneReg(0x3D    , 0xC8);
    ret = WriteOneReg(0xED    , 0x83);

    wait_ms(1000);

    ret = ReadOneReg(0xFA, &value);
    ret = ReadOneReg(0xFB, &result);

    if((result&0xC0) == 0xC0)
    {
        *rco = value;
         ChipInitReset();
         TRIM_EN = 0;
        return 0;
    }

    //==================Second BAND=====================//
    ChipInitReset();
    ret = WriteOneReg(0xED    , 0x03);
    ret = WriteOneReg(0xEC    , 0x36);
    ret = WriteOneReg(0x3D    , 0xC0);
    ret = WriteOneReg(0xE8    , 0x00);
    ret = WriteOneReg(0xEA    , 0x7F);
    ret = WriteOneReg(0xEB    , 0x40);
    ret = WriteOneReg(0x3D    , 0xC8);
    ret = WriteOneReg(0xED    , 0x83);

    wait_ms(1000);

    ret = ReadOneReg(0xFA, &value);
    ret = ReadOneReg(0xFB, &result);

    if((result&0xC0) == 0xC0)
    {
        *rco = value;
         ChipInitReset();
         TRIM_EN = 0;
        return 0;
    }    

    //==================Third BAND=====================//
    ChipInitReset();
    ret = WriteOneReg(0xED    , 0x03);
    ret = WriteOneReg(0xEC    , 0x36);
    ret = WriteOneReg(0x3D    , 0xC0);
    ret = WriteOneReg(0xE8    , 0x00);
    ret = WriteOneReg(0xEA    , 0xBF);
    ret = WriteOneReg(0xEB    , 0x80);
    ret = WriteOneReg(0x3D    , 0xC8);
    ret = WriteOneReg(0xED    , 0x83);

    wait_ms(1000);

    ret = ReadOneReg(0xFA, &value);
    ret = ReadOneReg(0xFB, &result);

    if((result&0xC0) == 0xC0)
    {
        *rco = value;
         ChipInitReset();
         TRIM_EN = 0;
        return 0;
    }    

    //==================Fourth BAND=====================//
    ChipInitReset();
    ret = WriteOneReg(0xED    , 0x03);
    ret = WriteOneReg(0xEC    , 0x36);
    ret = WriteOneReg(0x3D    , 0xC0);
    ret = WriteOneReg(0xE8    , 0x00);
    ret = WriteOneReg(0xEA    , 0xFF);
    ret = WriteOneReg(0xEB    , 0xC0);
    ret = WriteOneReg(0x3D    , 0xC8);
    ret = WriteOneReg(0xED    , 0x83);

    wait_ms(1000);

    ret = ReadOneReg(0xFA, &value);
    ret = ReadOneReg(0xFB, &result);

    if((result&0xC0) == 0xC0)
    {
        *rco = value;
         ChipInitReset();
         TRIM_EN = 0;
        return 0;
    }    

    ChipInitReset();
    TRIM_EN = 0;
    return 1;
}

uint8_t BVD_Trim(uint8_t *bvd)
{
    uint8_t ret = 0;
    uint32_t timeout = 0;
        
    WriteOneReg(0xC0, 0x00);    
    wait_ms(1);

    timeout = 600;
    int_mark = 1;
    WriteOneReg(0x0A, 0x08);
    while(int_mark == 1 && timeout != 0) {
        timeout--;
        wait_ms(100);
    }
    //osDelay(80);   
    
    ret = ReadOneReg(0xC0, bvd);

    if(timeout == 0)
        return 1;

    return 0;
}

uint8_t Pixel_Enable(uint8_t *buf)
{
    for(uint8_t i = 0; i < 18; i++)
    {
        WriteOneReg(0xC2 + i, buf[i]);  
    }
    
    return 0;
}