write_cmos_sensor(0x56, 0x07);
write_cmos_sensor(0x57, 0xF5);
write_cmos_sensor(0x58, 0xEA);
write_cmos_sensor(0x59, 0x21);

write_cmos_sensor(0x63, 0x90);
write_cmos_sensor(0x64, 0xB8);    
write_cmos_sensor(0x65, 0x90);    
write_cmos_sensor(0x66, 0xB8);      
write_cmos_sensor(0x67, 0xC8);    
write_cmos_sensor(0x68, 0x9F);      
write_cmos_sensor(0x69, 0xC8);    
write_cmos_sensor(0x6A, 0x9F);

//IDP
write_cmos_sensor(0x00, 0x03);
write_cmos_sensor(0x10, 0xEF);
write_cmos_sensor(0x11, 0x1D);  // PCLK, HSync, VSync (L,H,L)
write_cmos_sensor(0x12, 0x9D);  // YCbCr 
write_cmos_sensor(0x13, 0x00);
write_cmos_sensor(0x14, 0xF5);
write_cmos_sensor(0x15, 0x00);

//Shading
write_cmos_sensor(0x27, 0xFD); 
write_cmos_sensor(0x28, 0xCD);
write_cmos_sensor(0X29, 0xA8);
write_cmos_sensor(0x2A, 0x64);
write_cmos_sensor(0x2B, 0x21); 
write_cmos_sensor(0x2C, 0x44); 
write_cmos_sensor(0x2D, 0x24);
write_cmos_sensor(0X2E, 0x00);
write_cmos_sensor(0x2F, 0x00);
write_cmos_sensor(0x30, 0x00); 
write_cmos_sensor(0x31, 0x00);
write_cmos_sensor(0X32, 0xA1);
write_cmos_sensor(0x33, 0x81);

//Gamma
write_cmos_sensor(0x34, 0x00);                      
write_cmos_sensor(0x35, 0x06);                    
write_cmos_sensor(0x36, 0x11);            
write_cmos_sensor(0x37, 0x21);           
write_cmos_sensor(0x38, 0x3F);           
write_cmos_sensor(0x39, 0x59);          
write_cmos_sensor(0x3A, 0x6E);          
write_cmos_sensor(0x3B, 0x83);           
write_cmos_sensor(0x3C, 0x95);          
write_cmos_sensor(0x3D, 0xA6);           
write_cmos_sensor(0x3E, 0xb4);          
write_cmos_sensor(0x3F, 0xCE);           
write_cmos_sensor(0x40, 0xE5);          
write_cmos_sensor(0x41, 0xF5);          
write_cmos_sensor(0x42, 0xFC);           
write_cmos_sensor(0x43, 0xFF);

//DPC, De-noise
write_cmos_sensor(0x44, 0xBB);
write_cmos_sensor(0x45, 0x0E);
write_cmos_sensor(0x46, 0x17);
write_cmos_sensor(0x47, 0x20);
write_cmos_sensor(0x48, 0x20);	//De-noise Threshold
write_cmos_sensor(0x49, 0x4F);
write_cmos_sensor(0x4A, 0xFF);
write_cmos_sensor(0x4B, 0xFF);	//De-noise Start Gain
write_cmos_sensor(0x4C, 0xB1);
write_cmos_sensor(0x4D, 0x18);
write_cmos_sensor(0x4E, 0xFF);
write_cmos_sensor(0x4F, 0x10);	//LPF Threshold
write_cmos_sensor(0x50, 0x2F);
write_cmos_sensor(0x51, 0x40);	//Dark Start Again
write_cmos_sensor(0x52, 0x58);
write_cmos_sensor(0x53, 0x00);	//Edge detection

//CMA-D65
write_cmos_sensor(0x54, 0x3C);
write_cmos_sensor(0x55, 0xC3);
write_cmos_sensor(0x56, 0x02);
write_cmos_sensor(0x57, 0x0F);
write_cmos_sensor(0x58, 0x26);
write_cmos_sensor(0x59, 0x0B);
write_cmos_sensor(0x5A, 0xF1);
write_cmos_sensor(0x5B, 0xCE);
write_cmos_sensor(0x5C, 0x43);

//CMA-D30
write_cmos_sensor(0x5D, 0x46);
write_cmos_sensor(0x5E, 0xBE);
write_cmos_sensor(0x5F, 0xFD);
write_cmos_sensor(0x60, 0x02);
write_cmos_sensor(0x61, 0x29);
write_cmos_sensor(0x62, 0x15);
write_cmos_sensor(0x63, 0xFA);
write_cmos_sensor(0x64, 0xBC);
write_cmos_sensor(0x65, 0x4C);

//CMA-D20
write_cmos_sensor(0x66, 0x3D);
write_cmos_sensor(0x67, 0xC2);
write_cmos_sensor(0x68, 0x02);
write_cmos_sensor(0x69, 0xFD);
write_cmos_sensor(0x6A, 0x3B);
write_cmos_sensor(0x6B, 0x09);
write_cmos_sensor(0x6C, 0xFA);
write_cmos_sensor(0x6D, 0x9E);
write_cmos_sensor(0x6E, 0x6A);

write_cmos_sensor(0x6F, 0xC0);

//Edge
write_cmos_sensor(0x70, 0x60);	//Edge Type1 Upper Gain
write_cmos_sensor(0x71, 0x80);	//Edge Type1 Down Gain
write_cmos_sensor(0x73, 0x02);
write_cmos_sensor(0x74, 0x02);
write_cmos_sensor(0x75, 0x20);
write_cmos_sensor(0x76, 0x40);	//0x20 //Green Edge Suppress Start Again
write_cmos_sensor(0x77, 0xF0);	//Green Edge Slope, End Gain
write_cmos_sensor(0x78, 0x00);
write_cmos_sensor(0x79, 0x10);
write_cmos_sensor(0x7A, 0x30);
write_cmos_sensor(0x7B, 0x04);
write_cmos_sensor(0x7C, 0x04);
write_cmos_sensor(0x7D, 0x08);
write_cmos_sensor(0x7E, 0x08);
write_cmos_sensor(0x7F, 0x40);	//Luminance Edge Suppress Start Again
write_cmos_sensor(0x80, 0xF0);	//Luminance Edge Slope, End Gain
write_cmos_sensor(0x81, 0x00);
write_cmos_sensor(0x82, 0xFA);
write_cmos_sensor(0x83, 0x10);

//Contrast, Saturation
write_cmos_sensor(0x85, 0x12);
write_cmos_sensor(0x86, 0x12);
write_cmos_sensor(0x87, 0x12);
write_cmos_sensor(0x88, 0x00);

write_cmos_sensor(0x89, 0x00);
write_cmos_sensor(0x8A, 0x60);
write_cmos_sensor(0x8B, 0xA0);
write_cmos_sensor(0x8C, 0xFF);
write_cmos_sensor(0x8D, 0x10);
write_cmos_sensor(0x8E, 0xFF);
write_cmos_sensor(0x8F, 0x00);
write_cmos_sensor(0x90, 0xFF);
write_cmos_sensor(0x91, 0x00);

//Saturation Suppress
write_cmos_sensor(0x92, 0x40);	//Color Saturation Suppress Start Again
write_cmos_sensor(0x93, 0x2A);	//0x22 //Suppress Slop, End Gain
write_cmos_sensor(0x94, 0x40);
write_cmos_sensor(0x95, 0x00);
write_cmos_sensor(0x96, 0x58);
write_cmos_sensor(0x97, 0x40);
write_cmos_sensor(0x98, 0xEA);
write_cmos_sensor(0x99, 0xD3);
write_cmos_sensor(0x9A, 0x40);
write_cmos_sensor(0x9B, 0x6F);
write_cmos_sensor(0x9C, 0x00);
write_cmos_sensor(0x9D, 0x00);

//Windowing
write_cmos_sensor(0xA0, 0x24);
write_cmos_sensor(0xA1, 0x00);
write_cmos_sensor(0xA2, 0x80);
write_cmos_sensor(0xA3, 0x00);
write_cmos_sensor(0xA4, 0xE0);

//Sensor On
write_cmos_sensor(0x00, 0x00);
write_cmos_sensor(0x03, 0x05);

   
}   /* SIV120A_Write_Sensor_Initial_Setting */

/*************************************************************************
* FUNCTION
*   SIV120A_Init
*
* DESCRIPTION
*   This function initialize the registers of CMOS sensor and ISP control register.
*
* PARAMETERS
*   None
*
* RETURNS
*   None
*
* GLOBALS AFFECTED
*
*************************************************************************/
kal_int8 SIV120A_Init(void)
{
    /*----------------------------------------------------------------*/
    /* Local Variables                                                */
    /*----------------------------------------------------------------*/
    kal_uint16 sensor_id = 0;
    kal_uint8 iI;
	
    /*----------------------------------------------------------------*/
    /* Code Body                                                      */
    /*----------------------------------------------------------------*/
    /* PowerOn the sensor */
    cis_module_power_on(KAL_TRUE);
    kal_sleep_task(2);  /* delay for power stable */

    /* set sensors chip enable pin to low to activate SIV120A */
    PWRDN_PIN_HIGH;

    set_isp_driving_current(camera_para.SENSOR.reg[CMMCLK_CURRENT_INDEX].para);

    /* SIV100 needs 24M MCLK */
    /* Use UPLL to produce 48M clock, then divided by 2 */
    DISABLE_CAMERA_TG_CLK_48M;
    ENABLE_CAMERA_TG_CLK_48M;
    UPLL_Enable(UPLL_OWNER_ISP);
    SET_TG_OUTPUT_CLK_DIVIDER(1);
    SET_CMOS_RISING_EDGE(0);
    SET_CMOS_FALLING_EDGE(1);

    /* Sensor Signal Polarity */
    /* Note: Set HSync Polarity to Low means High active */
    ENABLE_CAMERA_PIXEL_CLKIN_ENABLE;
    SET_CMOS_CLOCK_POLARITY_LOW;
    SET_VSYNC_POLARITY_LOW;
    SET_HSYNC_POLARITY_LOW;

    /* YUV Sensor Input Format */
    ENABLE_CAMERA_INDATA_FORMAT;            /* Enable yuv data input */
    SET_CAMERA_INPUT_TYPE(INPUT_YUV422);    /* SIV120A support YUV422 */
    SET_CAMERA_INPUT_ORDER(INPUT_ORDER_CbYCrY1);

    /* Reset the sensor */
   // RESET_PIN_HIGH;
    //RESET_PIN_LOW;
   // for (iI = 0; iI < 0x80; iI++);
	//
   // RESET_PIN_HIGH;
    kal_sleep_task(2);  /* delay for stable sensor */

    // check sensor's device information

	write_cmos_sensor(0x00, 0x00);    
    if (read_cmos_sensor(0x01) != SIV120A_SENSOR_ID ||
        read_cmos_sensor(0x02) != SIV120A_SENSOR_VERSION) {
        return -1;
    }


    SIV120A_Write_Sensor_Initial_Setting();
    return 1;
	
}   /* SIV120A_Init() */

/*************************************************************************
* FUNCTION
*   ConfigVBlank
*
* DESCRIPTION
*   This function is to set VBlank size.
*
* PARAMETERS
*   iBlank: target VBlank size
*      iHz: banding frequency
* RETURNS
*   None
*
* GLOBALS AFFECTED
*
*************************************************************************/
void ConfigVBlank(const kal_uint16 iBlank, const kal_uint8 iHz)
{
    /********************************************
    *   50Hz-relative registers can not be used *
    ********************************************/
    const kal_uint16 iVal2Write = iBlank - 1;

    ASSERT(iBlank < (1 << 10));

    switch (iHz) {
    case CAM_BANDING_50HZ:
    case CAM_BANDING_60HZ:
	write_cmos_sensor(0x00,0x00);
        write_cmos_sensor(0x20, (read_cmos_sensor(0x20) & 0xFC) | ((iVal2Write & 0x0300) >> 8));
        write_cmos_sensor(0x23, iVal2Write & 0x00FF);
        break;

//    case CAM_BANDING_50HZ:
        // SIV120A cannot use all blank size registers of 50Hz
//        write_cmos_sensor(0x23, (read_cmos_sensor(0x23) & 0xFC) | ((iVal2Write & 0x0300) >> 8));
//        write_cmos_sensor(0x25, iVal2Write & 0x00FF);
    }
}   /* ConfigVBlank */

/*************************************************************************
* FUNCTION
*   ConfigHBlank
*
* DESCRIPTION
*   This function is to set HBlank size.
*
* PARAMETERS
*   iBlank: target HBlank size
*      iHz: banding frequency
* RETURNS
*   None
*
* GLOBALS AFFECTED
*
*************************************************************************/
void ConfigHBlank(const kal_uint16 iBlank, const kal_uint8 iHz)
{
    /********************************************
    *   50Hz-relative registers can not be used *
    ********************************************/
    const kal_uint16 iVal2Write = iBlank - 1;

    ASSERT(iBlank < (1 << 10));

    switch (iHz) {
    case CAM_BANDING_50HZ:
    case CAM_BANDING_60HZ:
	write_cmos_sensor(0x00,0x00);
        write_cmos_sensor(0x20, (read_cmos_sensor(0x20) & 0xCF) | ((iVal2Write & 0x0300) >> 4));
        write_cmos_sensor(0x21, iVal2Write & 0x00FF);
        break;

//    case CAM_BANDING_50HZ:
        // SIV120A cannot use all blank size registers of 50Hz
//        write_cmos_sensor(0x23, (read_cmos_sensor(0x23) & 0xCF) | ((iVal2Write & 0x0300) >> 4));
//        write_cmos_sensor(0x24, iVal2Write & 0x00FF);
    }
}   /* ConfigHBlank */

/*************************************************************************
* FUNCTION
*   SIV120A_PowerOff
*
* DESCRIPTION
*   This function is to turn off sensor module power.
*
* PARAMETERS
*   None
*
* RETURNS
*   None
*
* GLOBALS AFFECTED
*
*************************************************************************/
void SIV120A_PowerOff(void)
{
	write_cmos_sensor(0x00, 0x00);
	write_cmos_sensor(0x03, 0x02);	//sensor sleep mode

    cis_module_power_on(KAL_FALSE); // power off sensor

    SET_SCCB_CLK_LOW;
    SET_SCCB_DATA_LOW;

    UPLL_Disable(UPLL_OWNER_ISP);
}   /* SIV120A_PowerOff */

/*************************************************************************
* FUNCTION
*   SIV120A_Get_I2C_ID
*
* DESCRIPTION
*   This function return the sensor read/write IDs of I2C I/F.
*
* PARAMETERS
*   *pI2C_WriteID : address pointer of sensor's I2C write id
*   *pI2C_ReadID  : address pointer of sensor's I2C read id
*
* RETURNS
*   None
*
* GLOBALS AFFECTED
*
*************************************************************************/
void SIV120A_Get_I2C_ID(kal_uint8 *pI2C_WriteID, kal_uint8 *pI2C_ReadID)
{
    *pI2C_WriteID = SIV120A_I2C_WRITE_ID;
    *pI2C_ReadID = SIV120A_I2C_READ_ID;
}   /* SIV120A_Get_I2C_ID */

/*************************************************************************
* FUNCTION
*   SIV120A_GetSize
*
* DESCRIPTION
*   This function return the image width and height of image sensor.
*
* PARAMETERS
*   *pWidth : address pointer of horizontal effective pixels of image sensor
*  *pHeight : address pointer of vertical effective lines of image sensor
*
* RETURNS
*   None
*
* GLOBALS AFFECTED
*
*************************************************************************/
void SIV120A_GetSize(kal_uint16 *pWidth, kal_uint16 *pHeight)
{
    *pWidth = IMAGE_SENSOR_VGA_WIDTH;   // valid pixel # in one frame
    *pHeight = IMAGE_SENSOR_VGA_HEIGHT; // valid line # in one frame
}   /* SIV120A_GetSize */

/*************************************************************************
* FUNCTION
*   SIV120A_GetPeriod
*
* DESCRIPTION
*   This function return the image width and height of image sensor.
*
* PARAMETERS
*   *pPixelNumber : address pointer of pixel numbers in one period of HSYNC
*   *pLineNumber : address pointer of line numbers in one period of VSYNC
*
* RETURNS
*   None
*
* GLOBALS AFFECTED
*
*************************************************************************/
void SIV120A_GetPeriod(kal_uint16 *pPixelNumber, kal_uint16 *pLineNumber)
{
    /************************************
    *   No need for YUV sensor driver   *
    ************************************/
//    *pPixelNumber = VGA_PERIOD_PIXEL_NUMS;  // pixel numbers in one period of HSYNC
//    *pLineNumber = VGA_PERIOD_LINE_NUMS;    // line numbers in one period of VSYNC
}   /* SIV120A_GetPeriod */

/*************************************************************************
* FUNCTION
*	SIV120A_Preview
*
* DESCRIPTION
*	This function configure the sensor to preview mode.
*
* PARAMETERS
*
* RETURNS
*	None
*
* GLOBALS AFFECTED
*
*************************************************************************/
void SIV120A_Preview(image_sensor_exposure_window_struct *pImage_Window, image_sensor_config_struct *pSensor_Config_Data)
{
    kal_uint8 iHV_Mirror = 0;
    kal_uint8 iStartX = 0, iStartY = 1;
    kal_uint8 iTemp = 0;
    volatile kal_uint16 iRetry;
    static kal_uint16 iBound = 0x00FF;

    
    if (pSensor_Config_Data->isp_op_mode == ISP_MJPEG_PREVIEW_MODE
        || pSensor_Config_Data->isp_op_mode == ISP_MJPEG_ENCODE_MODE) { // MJPEG mode
        g_bVideoMode = KAL_TRUE;
		g_bPreviewMode = KAL_TRUE;		

        // config TG for video recorder mode
        SET_TG_OUTPUT_CLK_DIVIDER(3);      // MCLK = 12MHz
        SET_CMOS_RISING_EDGE(0);
        SET_CMOS_FALLING_EDGE(2);
        SET_TG_PIXEL_CLK_DIVIDER(3);
        SET_CMOS_DATA_LATCH(2);