/*************************************************************************************************
// INTEL CONFIDENTIAL Copyright 2011-2020 Intel Corporation All Rights Reserved.
//
// The source code contained or described herein and all documents related to the source code
// ("Material") are owned by Intel Corporation or its suppliers or licensors. Title to the Material
// remains with Intel Corporation or its suppliers and licensors. The Material contains trade secrets
// and proprietary and confidential information of Intel or its suppliers and licensors. The Material is
// protected by worldwide copyright and trade secret laws and treaty provisions. No part of the
// Material may be used, copied, reproduced, modified, published, uploaded, posted, transmitted,
// distributed, or disclosed in any way without Intel's prior express written permission.
//
// No license under any patent, copyright, trade secret or other intellectual property right is
// granted to or conferred upon you by disclosure or delivery of the Materials, either expressly, by
// implication, inducement, estoppel or otherwise. Any license under such intellectual property
// rights must be express and approved by Intel in writing.
//*************************************************************************************************/

#include "device.h"
#include "SensorTrace.h"
#include "Sample.tmh"
#include "Ssreader.h"
#include "Sample_setting.c"

#define INITGUID

// Sample MIPI config
const SNSR_MIPI_CONFIG DefaultSensorMipiConfig =
{
    MIPI_CSI2_PORT_0,
    MIPI_LANES_4,
    INPUT_FORMAT_RAW_10,
    TYPE_RAW,
};

SNSR_CAPS DefaultSensorCaps =
{
    MIPI,
    RAW,
    GET_RATIO(16, 9),       // Aspect ratio
    0,      0,              // Aperture
    65000,  1,              // Exposure
    800,    50,             // ISO
    2000,  100,   2000,     // Focus min/max/default value - in millimeters
    TRUE, // Focus support
    TRUE, // Flash support
    FALSE, // Scale support
    29,                    // FOV FocalLengthX in mm
    26,                    // FOV FocalLengthY in mm
    0,                     // Camera Angle Offset pitch angle
    0,                     // Camera Angle Offset yaw angle
    1,                     // Embedded Line number 1
    1,
#ifdef AE_EXACT_CONFIG
    4,
    4,                     // ae store
#endif
};

#ifdef AE_EXACT_CONFIG
const LONG ExpEffectDelayFrames[3]=
{
    3, //exposure time
    3, //analog gain
    2, //digital gain
};
#endif

SENSOR_RESOLUTION sensor_res_preview[] =
{
    {3280, 1848, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_3280x1848_setting},
    {3280, 2464, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_3280x2464_setting},
    RESOLUTION_TABLE_END
};

SENSOR_RESOLUTION sensor_res_still[] =
{
    {3280, 1848, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_3280x1848_setting},
    {3280, 2464, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_3280x2464_setting},
    RESOLUTION_TABLE_END
};

SENSOR_RESOLUTION sensor_res_video[] =
{
    {1932, 1092, 30, BINNING1x1, 720000000, BGGR, BGGR, 0, 0, (REGS_SETTING *)sensor_1932x1092_setting},
    {2124, 1200, 30, BINNING1x1, 720000000, BGGR, BGGR, 0, 0, (REGS_SETTING *)sensor_2124x1200_setting},
    {3280, 1848, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_3280x1848_setting},
    {3280, 2464, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_3280x2464_setting},
    RESOLUTION_TABLE_END
};

SENSOR_RESOLUTION sensor_res_raw[] =
{
    {1932, 1092, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_1932x1092_setting},
    {2124, 1200, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_2124x1200_setting},
    {3280, 1848, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_3280x1848_setting},
    {3280, 2464, 30, BINNING1x1, 720000000, BGGR, BGGR, 1, 0, (REGS_SETTING *)sensor_3280x2464_setting},
    RESOLUTION_TABLE_END
};


NTSTATUS GetTimingFactor(
    PDEVICE_CONTEXT SensorCtx,
    PSENSOR_MODE_DATA pModeData
)
{
    NTSTATUS    status = STATUS_SUCCESS;
    ULONG data;
    ULONG data1;
    unsigned int pll_multiplier;
    unsigned int pre_pll2_clk_div0;
    
    //unsigned int dac_divider;
    unsigned int pll2_sys_pre_div;
    unsigned int pll2_sclk_pdiv;
    unsigned int pll2_sys_pre_div2;
    
    unsigned int pre_pll2_clk_div0_num[2] = {1,2}; 
    float pre_pll2_clk_div;
    float pre_pll2_clk_div_num[8] = {1, 1.5, 2, 2.5, 3, 4, 6, 8};
    unsigned int pll2_sys_pre_div_num[4] = {1,2,4,1};
    unsigned int pll2_sclk_pdiv_num[16] = {1,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
    float pre_sys_divider;
    float pre_sys_divider_val[8] = { 1, 1.5, 2, 2.5, 3, 3.5, 4, 5 };
    const unsigned int ext_clk_freq_hz = 19200000;
    UINT16 FrameInfo[MODE_DATA_FRAME_NUM] = {0};

    CHECK_POINTER_INVALID(SensorCtx);
    CHECK_POINTER_INVALID(pModeData);

    // 1. frame information
    status = RegReadBuf(&SensorCtx->I2cDev, MODE_DATA_FRAME_ADDR, (UINT8*)FrameInfo, MODE_DATA_FRAME_NUM * 2);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s fail to read frame information (0x%x)\n", DEVICE_NAME, status);
        goto done;
    }

    ConvertEndian(FrameInfo, sizeof(FrameInfo) / sizeof(FrameInfo[0]));


    pModeData->CropHorizontalStart = FrameInfo[(REG_H_START - MODE_DATA_FRAME_ADDR)/2];
    pModeData->CropVerticalStart   = FrameInfo[(REG_V_START - MODE_DATA_FRAME_ADDR)/2];
    pModeData->CropHorizontal_end  = FrameInfo[(REG_H_END - MODE_DATA_FRAME_ADDR)/2];
    pModeData->CropVertical_end    = FrameInfo[(REG_V_END - MODE_DATA_FRAME_ADDR)/2];
    pModeData->OutputWidth         = FrameInfo[(REG_WIDTH - MODE_DATA_FRAME_ADDR)/2];
    pModeData->OutputHeight        = FrameInfo[(REG_HEIGHT - MODE_DATA_FRAME_ADDR)/2];
    pModeData->LineLengthPck       = FrameInfo[(REG_TIMING_HTS - MODE_DATA_FRAME_ADDR)/2];
    pModeData->FrameLengthLines    = FrameInfo[(REG_TIMING_VTS - MODE_DATA_FRAME_ADDR)/2];
    pModeData->FineIntegrationTimeMin = 0;
    pModeData->FineIntegrationTimeMaxMargin = pModeData->LineLengthPck;
    pModeData->CoarseIntegrationTimeMin = 1;
    pModeData->CoarseIntegrationTimeMaxMargin = 4;

    DoTraceMessage(FLAG_LOG, "%s hts=0x%x, vts=0x%x", DEVICE_NAME, pModeData->LineLengthPck, pModeData->FrameLengthLines);
    DoTraceMessage(FLAG_LOG, "%s hout=0x%x, vout=0x%x", DEVICE_NAME, pModeData->OutputWidth, pModeData->OutputHeight);
    DoTraceMessage(FLAG_LOG, "%s hstart=0x%x, hend=0x%x", DEVICE_NAME, pModeData->CropHorizontalStart, pModeData->CropHorizontal_end);
    DoTraceMessage(FLAG_LOG, "%s vstart=0x%x, vend=0x%x", DEVICE_NAME, pModeData->CropVerticalStart, pModeData->CropVertical_end);
    DoTraceMessage(FLAG_LOG, "%s fineMin=0x%x, fineMargin=0x%x", DEVICE_NAME, pModeData->FineIntegrationTimeMin, pModeData->FineIntegrationTimeMaxMargin);
    DoTraceMessage(FLAG_LOG, "%s coarseMin=0x%x, coarseMargin=0x%x", DEVICE_NAME, pModeData->CoarseIntegrationTimeMin, pModeData->CoarseIntegrationTimeMaxMargin);

    
    // 2. binning factor
    status = RegRead(SensorCtx, REG_TIMING_X_INC, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
      goto done;
    pModeData->BinningFactorX = (((UINT8) (data & 0x0f))  + 1) / 2;
    if (0 == pModeData->BinningFactorX)
      pModeData->BinningFactorX = 1;

    status = RegRead(SensorCtx, REG_TIMING_Y_INC, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
      goto done;
    pModeData->BinningFactorY = (((UINT8) (data & 0x0f))  + 1) / 2;
    if (0 == pModeData->BinningFactorY)
      pModeData->BinningFactorY = 1;

    SensorCtx->Binning = pModeData->BinningFactorX;

    DoTraceMessage(FLAG_LOG, "%s hbinning=0x%x, vbinning=0x%x", DEVICE_NAME, pModeData->BinningFactorX, pModeData->BinningFactorY);

    
    // 3. clock information
 
    status = RegRead(SensorCtx, REG_PLL2_PREDIV0, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    pre_pll2_clk_div0 = pre_pll2_clk_div0_num[(data>>4) & 0x1];


    status = RegRead(SensorCtx, REG_PLL2_PREDIV, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    pre_pll2_clk_div = pre_pll2_clk_div_num[data & 0x7];

    //dac_divider = 1 + data& 0xf;

    status = RegRead(SensorCtx, REG_PLL2_MULTIPLIER_H, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    status = RegRead(SensorCtx, REG_PLL2_MULTIPLIER_L, &data1, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    pll_multiplier = (((data & 0x3) << 8)|(data1 & 0xff));


    status = RegRead(SensorCtx, REG_PLL2_SYS_PRE_DIV2, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    pll2_sys_pre_div2 = ((data & 0xf)+1);

        status = RegRead(SensorCtx, REG_PLL2_SYS_DIVIDER, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    pre_sys_divider = pre_sys_divider_val[data & 0x7];

    status = RegRead(SensorCtx, REG_PLL2_SYS_PRE_DIV, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    pll2_sys_pre_div = pll2_sys_pre_div_num[(data >> 2) & 0x3];

    pll2_sclk_pdiv = pll2_sclk_pdiv_num[( data >> 4) & 0xf];
 
    pModeData->VtPixClkFreqHz = (unsigned int)(ext_clk_freq_hz / pre_pll2_clk_div0 / pre_pll2_clk_div* pll_multiplier / pll2_sys_pre_div2 / pre_sys_divider / pll2_sys_pre_div / pll2_sclk_pdiv);

#ifdef AE_EXACT_CONFIG
    {
        LONG vts, hts, height, pclk;
        vts = (LONG)(pModeData->FrameLengthLines);
        hts = (LONG)(pModeData->LineLengthPck);
        height = (LONG)(pModeData->OutputHeight);
        pclk = (LONG)(pModeData->VtPixClkFreqHz);
        
        SensorCtx->CurPixClkFreqHz = (LONG)(pModeData->VtPixClkFreqHz);    
        SensorCtx->DefaultHTS = (LONG)(pModeData->LineLengthPck);
        SensorCtx->DefaultVTS = (LONG)(pModeData->FrameLengthLines);

        if (pclk == 0)
            SensorCtx->DefaultDelay = 10;
        else
            SensorCtx->DefaultDelay = (LONG)((vts - height)*hts/(pclk/1000)); //ms
        if (SensorCtx->DefaultDelay == 0)
            SensorCtx->DefaultDelay = 10;
    }
#endif
    DoTraceMessage(FLAG_LOG, "%s p_clk=%d", DEVICE_NAME, pModeData->VtPixClkFreqHz);

done:
invalid:
    return status;
}

static NTSTATUS GetMipiBps(
        PDEVICE_CONTEXT SensorCtx,
        INT32 *pMipiBps
        )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG data = 0;
    ULONG pre_pll1_perdiv0;
    ULONG pll1_m_divider;
    ULONG pll1_multiplier;
    ULONG data1 = 0;
    ULONG MipiBps;
    const unsigned int ext_clk_freq_hz = 19200000;

    float pre_pll1_perdiv;
    float pre_pll1_perdiv_val[8]= {1, 1.5, 2, 2.5, 3, 4, 6, 8};

    status = RegRead(SensorCtx, REG_PLL1_PREDIV0, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    pre_pll1_perdiv0 = ((data & 0x1) + 1);


    status = RegRead(SensorCtx, REG_PLL1_PREDIV, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    pre_pll1_perdiv = pre_pll1_perdiv_val[data & 0x7];

    status = RegRead(SensorCtx, REG_PLL1_MULTIPLIER_H, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    status = RegRead(SensorCtx, REG_PLL1_MULTIPLIER_L, &data1, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    pll1_multiplier = (((data & 0x3) << 8 ) | data1);

    status = RegRead(SensorCtx, REG_PLL1_M_DIVIDER, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    pll1_m_divider = ((data & 0xf) +1);

    MipiBps = (ULONG)(ext_clk_freq_hz / pre_pll1_perdiv0 / pre_pll1_perdiv * pll1_multiplier / pll1_m_divider);
    DoTraceMessage(FLAG_LOG, "%s MipiBps=%d", DEVICE_NAME, MipiBps);
    *pMipiBps = (INT32)MipiBps;
done:
    return status;
}

void InitContext(PDEVICE_CONTEXT deviceCtx)
{
    DoTraceMessage(FLAG_LOG, "%s InitContext\n", DEVICE_NAME);

#if defined(OTM_SENSOR_CONTROL_LOGIC)
    deviceCtx->SensorConf.Mclk = deviceCtx->SensorDriverView.SensorDriverSubView.Mclk;
    deviceCtx->SensorConf.Pld = deviceCtx->Pld.Panel;
    deviceCtx->SensorConf.volConf.VA = 2800;
    deviceCtx->SensorConf.volConf.VIO = 1800;
    deviceCtx->SensorConf.volConf.VCM = 2800;
    deviceCtx->SensorConf.volConf.VD = 1200;
    deviceCtx->SensorConf.volConf.V_SIO = 1800;
#endif

    deviceCtx->Guid = SENSOR_GUID;

    SsI2cSetAddrBits(&deviceCtx->I2cDev, I2C_GENERAL, ADDR_16BITS);
    deviceCtx->SensorInterface.SetSendInputImageIntf    = NULL;
    deviceCtx->SensorInterface.SendImageToFIFO          = NULL;
    deviceCtx->SensorInterface.GetVersion               = Cmd_GetVersion;
    deviceCtx->SensorInterface.GetSensorInfo            = Cmd_GetSensorInfo;
    deviceCtx->SensorInterface.GetMIPIConfigs           = Cmd_GetMIPIConfigs;
    deviceCtx->SensorInterface.GetCaps                  = Cmd_GetCaps;
    deviceCtx->SensorInterface.GetPld                   = Cmd_GetPld;
    deviceCtx->SensorInterface.GetResolutions           = Cmd_GetResolutions;
    deviceCtx->SensorInterface.SetResolution            = Cmd_SetResolution;
    deviceCtx->SensorInterface.StartViewfinder          = Cmd_StartViewfinder;
    deviceCtx->SensorInterface.StopViewfinder           = Cmd_StopViewfinder;
    deviceCtx->SensorInterface.GetModeData              = Cmd_GetModeData;
    deviceCtx->SensorInterface.SetIdle                  = Cmd_Idle;
    deviceCtx->SensorInterface.SetActive                = Cmd_Active;
    deviceCtx->SensorInterface.GetBinningMode           = Cmd_GetBinningMode;
    deviceCtx->SensorInterface.GetFrameRate             = Cmd_GetFrameRate;
    deviceCtx->SensorInterface.GetExtFeature            = Cmd_GetExtFeature;
    deviceCtx->SensorInterface.SignalVBlank             = NULL;
    deviceCtx->SensorInterface.SetExposure              = LocalCmd_SetExposure;
    deviceCtx->SensorInterface.GetExposure              = LocalCmd_GetExposure;
    deviceCtx->SensorInterface.GetGain                  = LocalCmd_GetGain;
    deviceCtx->SensorInterface.SetRegister              = Cmd_WriteReg;
    deviceCtx->SensorInterface.GetRegister              = Cmd_ReadReg;
    deviceCtx->SensorInterface.SetExtFeature            = NULL;

    if (deviceCtx->Key_EnableEmbeddedData == REGISTRY_INVALID)
        deviceCtx->IsEmbeddedDataEnabled = TRUE;
    else
        deviceCtx->IsEmbeddedDataEnabled = deviceCtx->Key_EnableEmbeddedData;

#if defined(OTM_SENSOR_CONTROL_LOGIC)
    deviceCtx->SensorInterface.GetControlLogicInfo      = Cmd_GetControlLogicInfo;
#else
    deviceCtx->SensorInterface.GetControlLogicInfo      = NULL;
#endif
}

NTSTATUS Detect(
    PDEVICE_CONTEXT SensorCtx,
    SNSR_VERSION_INFO *Version
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG data = 0;

    CHECK_POINTER_INVALID(SensorCtx)
    CHECK_POINTER_INVALID(Version)

    status = KeWaitForSingleObject(&SensorCtx->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s Detect KeWaitForSingleObject fail, (status 0x%x)\n",
            DEVICE_NAME, status);
        goto invalid;
    }

    // Sensor ID
    status = RegRead(SensorCtx, REG_SENSOR_ID, &data, DATA_16BITS);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s senser id  = 0x%x\n", DEVICE_NAME, data);
        goto done;
    }
    Version->SensorId = (UINT16)data;

    if (Version->SensorId != SENSOR_ID && Version->SensorId != SENSOR_ID_EX)
    {
        status = STATUS_UNSUCCESSFUL;
        DoTraceMessage(FLAG_LOG, "%s detect error 0x%x\n", DEVICE_NAME, Version->SensorId);
        goto done;
    }
    DoTraceMessage(FLAG_LOG, "%s detect sucessful 0x%x\n", DEVICE_NAME, Version->SensorId);

     // Sensor Revision
    status = RegRead(SensorCtx, REG_SENSOR_VERSION, &data, DATA_16BITS);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s senser version = 0x%x\n", DEVICE_NAME, data);
        goto done;
    }
    Version->SensorRevision= (UINT16)data;
    DoTraceMessage(FLAG_LOG, "%s detect successful 0x%x\n", DEVICE_NAME, Version->SensorRevision);

    //InitExportResTable(SensorCtx);

done:
    KeSetEvent(&SensorCtx->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    return status;
}


NTSTATUS InitCmos(
    PDEVICE_CONTEXT SensorCtx
    )
{
    UNREFERENCED_PARAMETER(SensorCtx);

    // Note: the init operation is done in InitResolution()
    
    return STATUS_SUCCESS;
}

NTSTATUS InitOtp(
    PNVM_CONTEXT pSsNvm
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    if (!pSsNvm)
    {
        status = STATUS_INVALID_PARAMETER;
        return status;
    }

    status = Cmd_OtpRead(pSsNvm, 0, pSsNvm->NvmBuffer, OTP_VALID_SIZE);
   
    return status;
}

NTSTATUS EnableEmbeddedLine(
    PDEVICE_CONTEXT SensorCtx
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG data = 0;

    // set 0x4307[0] to 1
    status = RegRead(SensorCtx, REG_EMBED_CTRL, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    data |= EMBED_EN;

    status = RegWrite(SensorCtx, REG_EMBED_CTRL, data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    DoTraceMessage(FLAG_LOG, "%s EnableEmbeddedLine Reg[0x%x]:0x%x", DEVICE_NAME, REG_EMBED_CTRL, data);

    // set 0x5A08[2] to 0: at the start of frame, 1: at the end of frame
    status = RegRead(SensorCtx, EMBED_FLAG, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    data &= 0xFB;
    status = RegWrite(SensorCtx, EMBED_FLAG, data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    // set 0x5041[0] to 1
    status = RegRead(SensorCtx, DSP_CTRL, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    data |= 1;
    status = RegWrite(SensorCtx, DSP_CTRL, data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    DoTraceMessage(FLAG_LOG, "%s EnableEmbeddedLine Reg[0x%x]:0x%x", DEVICE_NAME, DSP_CTRL, data);

    //Set 0x4816[5:0] for data type
    status = RegRead(SensorCtx, REG_EMB_DT_SEL, &data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    data &= 0xC0;
    data |= DATA_TYPE_EMBEDDED;
    status = RegWrite(SensorCtx, REG_EMB_DT_SEL, data, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;
    DoTraceMessage(FLAG_LOG, "%s EnableEmbeddedLine Reg[0x%x]:0x%x", DEVICE_NAME, REG_EMB_DT_SEL, data);

done:
    DoTraceMessage(FLAG_LOG, "%s EnableEmbeddedLine status 0x%x", DEVICE_NAME, status);
    return status;
}

NTSTATUS InitResolution(
    PDEVICE_CONTEXT SensorCtx,
    SNSR_MODE Mode,
    INT32 idx
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG data = 0;
    UNREFERENCED_PARAMETER(Mode);
    UNREFERENCED_PARAMETER(idx);

    SensorCtx->Streaming = FALSE;
    SensorCtx->QueueAnalogGain = REG_EXPGAIN_INVALID;
    SensorCtx->QueueAnalogGainDelay = REG_EXPGAIN_INVALID;
    SensorCtx->QueueDigitalGain = REG_EXPGAIN_INVALID;
    SensorCtx->QueueDigitalGainDelay = REG_EXPGAIN_INVALID;
    SensorCtx->QueueCoarse = REG_EXPGAIN_INVALID;
    SensorCtx->AERegValueCache.FLinesValid = FALSE;
    SensorCtx->AERegValueCache.CoarseValid = FALSE;
    SensorCtx->AERegValueCache.AnalogGainValid = FALSE;
    SensorCtx->AERegValueCache.DigitalGainValid = FALSE;

    // SW Reset Sensor:
    status = RegWriteQueue(SensorCtx, Sample_reset);

    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s Failed to reset (0x%x)\n", DEVICE_NAME, status);
        goto done;
    }
    SleepMSecond(1);
    // stream off
    status = RegWriteQueue(SensorCtx, Sample_soft_standby);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s Failed to stream off (0x%x)\n", DEVICE_NAME, status);
        goto done;
    }

    // global setting
    status = RegRead(SensorCtx, REG_SENSOR_VERSION, &data, DATA_16BITS);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s senser version = 0x%x\n", DEVICE_NAME, data);
        goto done;
    }

    status = RegWriteQueue(SensorCtx, sensor_global_setting);
    DoTraceMessage(FLAG_LOG, "%s is using global setting", DEVICE_NAME);

    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s Failed to global set (0x%x)\n", DEVICE_NAME, status);
        goto done;
    }
    // resolution setting
    status = RegWriteQueue(SensorCtx, Sensor_Res[Mode][idx].regs);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s Failed to resolution set (0x%x)\n", DEVICE_NAME, status);
        goto done;
    }

    status = GetMipiBps(SensorCtx, &Sensor_Res[Mode][idx].MipiBps);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s Failed to get mipi bps (0x%x)\n", DEVICE_NAME, status);
        goto done;
    }


    if(SensorCtx->Orientation == DEGREE_180)
    {
        ULONG flipReg, mirrorReg;
        status = RegRead(SensorCtx, REG_FLIP_CONTROL, &flipReg, DATA_8BITS);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "REG_FLIP_CONTROL Fail = 0x%x\n", flipReg);
            goto done;
        }
        status = RegRead(SensorCtx, REG_MIRROR_CONTROL, &mirrorReg, DATA_8BITS);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "REG_MIRROR_CONTROL Fail = 0x%x\n", mirrorReg);
            goto done;
        }
        flipReg ^= CONFIG_FLIP;
        mirrorReg ^= CONFIG_MIRROR;
        status = RegWrite(SensorCtx, REG_FLIP_CONTROL, flipReg, DATA_8BITS);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "REG_FLIP_CONTROL Fail = 0x%x\n", flipReg);
            goto done;
        }
        status = RegWrite(SensorCtx, REG_MIRROR_CONTROL, mirrorReg, DATA_8BITS);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "REG_MIRROR_CONTROL Fail = 0x%x\n", mirrorReg);
            goto done;
        }
    }

    if (SensorCtx->IsEmbeddedDataEnabled && DefaultSensorCaps.EmbeddedLineNum != 0)
    {
        status = EnableEmbeddedLine(SensorCtx);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s Failed to enable embedded line (0x%x)\n", DEVICE_NAME, status);
            goto done;
        }
    }
done:
    return status;
}


NTSTATUS Stream(
    PDEVICE_CONTEXT SensorCtx,
    BOOLEAN flag
    )
{
    NTSTATUS status = STATUS_UNSUCCESSFUL;

    if (!flag)
    {
        status = RegWriteQueue(SensorCtx, Sample_soft_standby);
#if defined(OTM_SENSOR_CONTROL_LOGIC)
        if (NT_SUCCESS(status) &&
            (SensorCtx->SKU.crd == SKU_CRD_G || SensorCtx->SKU.crd == SKU_CRD_G2  || SensorCtx->SKU.crd == SKU_CRD_D))
        {
            status = SensorCtx->ControlLogicInterface->PrivacyLedOff(
                SensorCtx->ControlLogicInterface->InterfaceHeader.Context, &SensorCtx->SensorConf);
        }
#endif
        return status;
    }

    status = RegWriteQueue(SensorCtx, Sample_streaming);
#if defined(OTM_SENSOR_CONTROL_LOGIC)
    if (NT_SUCCESS(status) &&
        (SensorCtx->SKU.crd == SKU_CRD_G || SensorCtx->SKU.crd == SKU_CRD_G2  || SensorCtx->SKU.crd == SKU_CRD_D))
    {
        status = SensorCtx->ControlLogicInterface->PrivacyLedOn(
            SensorCtx->ControlLogicInterface->InterfaceHeader.Context, &SensorCtx->SensorConf);
    }
#endif
    return status;
}

NTSTATUS ModuleCheck(
    PDEVICE_CONTEXT SensorCtx
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    SNSR_VERSION_INFO Version;
    USHORT                    data;

    // cmos check
    status = Detect(SensorCtx, &Version);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s cmos check Fail!\n", DEVICE_NAME);
        goto done;
    }
   
    // vcm check
    if (SensorCtx->SsVcm.VcmType != VCM_NONE)
    {
        status = SensorCtx->SensorInterface.SetFocusPos(SensorCtx, SensorCtx->SsVcm.Focus);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s vcm check Fail!\n", DEVICE_NAME);
            goto done;
        }

        status = SensorCtx->SensorInterface.GetFocusPos(SensorCtx, &data);
        //DoTraceMessage(FLAG_ERROR, "%s VCM read-setting = [%d]!\n", DEVICE_NAME, data);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s vcm get Fail!\n", DEVICE_NAME);
            goto done;
        }

    }

done:
    return status;
}

void ReleaseI2c(
    PDEVICE_CONTEXT SensorCtx
    )
{
    NTSTATUS status = STATUS_SUCCESS;    
    status = KeWaitForSingleObject(&SensorCtx->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s ReleaseI2c KeWaitForSingleObject fail, (status 0x%x)\n",
            DEVICE_NAME, status);
        goto invalid;
    }

    // comment: even wait event fail, it is better to still release i2c resource 
    SsI2cSlaveUninit(&SensorCtx->I2cDev, I2C_GENERAL);
    SsI2cSlaveUninit(&SensorCtx->I2cDev, I2C_VCM);
    KeSetEvent(&SensorCtx->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    return;
}


/// Public API
NTSTATUS
LocalCmd_SetExposure(
    PDEVICE_CONTEXT SensorCtx,
    SNSR_EXPOSURE ExposureTime
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG real;
    ULONG FLines;
    ULONG FrameLengthLines = ExposureTime.FrameLengthLines;
    UINT16 buf[4];
    ULONG DigitalGain;

    CHECK_POINTER_INVALID(SensorCtx)
        
    status = KeWaitForSingleObject(&SensorCtx->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SetExposure KeWaitForSingleObject fail, (status 0x%x)\n",
            DEVICE_NAME, status);
        goto invalid;
    }

    DigitalGain = ExposureTime.DigitalGain; //input is Q10 unity is 1024
    DigitalGain = DigitalGain & 0x0FFF; //12bit, 1x <= DG < 4x

    // set frame length
    real = ExposureTime.CoarseIntegrationTime;


    FLines = Sensor_Res[SensorCtx->Mode][SensorCtx->ResIndex].ExtendOpt[RES_EXTEND_VTS];

    if (!NT_SUCCESS(status))
        goto done;

    if (FLines < real + 4)
        FLines = real + 4;

    if (FLines < FrameLengthLines)
        FLines = FrameLengthLines;

    FLines = (FLines / 2 + 1) * 2;

    status = RegWrite(SensorCtx, REG_TIMING_VTS, FLines, DATA_16BITS);
    if (!NT_SUCCESS(status))
        goto done;

    // set exposure time
    real = real << 4;
    status = RegWrite(SensorCtx, REG_EXPOSURE, real, DATA_24BITS);
    if (!NT_SUCCESS(status))
        goto done;

    // set analog gain
    //group hold start (max 16 registers)
    status = RegWrite(SensorCtx, REG_GROUP_ACCESS, 0x00, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    real = ExposureTime.AnalogGain;
    status = RegWrite(SensorCtx, REG_ANA_GAIN, real, DATA_16BITS);
    if (!NT_SUCCESS(status))
        goto done;

    // set digital gain
    buf[0] = REG_MWB_GAIN_R;
    buf[1] = (UINT16)DigitalGain;
    buf[2] = (UINT16)DigitalGain;
    buf[3] = (UINT16)DigitalGain;

    status = RegWriteBuf(&SensorCtx->I2cDev, ConvertEndian(buf, sizeof(buf) / sizeof(buf[0])), sizeof(buf));

    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s fail to set digital gain\n", DEVICE_NAME);
        goto done;
    }

    //group hold end
    status = RegWrite(SensorCtx, REG_GROUP_ACCESS, 0x10, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    //group hold launch
    status = RegWrite(SensorCtx, REG_GROUP_ACCESS, 0xA0, DATA_8BITS);  
    if (!NT_SUCCESS(status))
        goto done;

done:    
    KeSetEvent(&SensorCtx->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, 
        "%s SampleCmd_SetExposure:Coarse (%d), AnalogGain (%d) DigitalGain (%d)  returns with code: 0x%x\n", DEVICE_NAME, 
        ExposureTime.CoarseIntegrationTime, ExposureTime.AnalogGain, ExposureTime.DigitalGain, status);
    return status;
}

NTSTATUS
LocalCmd_GetExposure(
    PDEVICE_CONTEXT SensorCtx,
    SNSR_EXPOSURE *Exposure
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG data;

    CHECK_POINTER_INVALID(SensorCtx)
    CHECK_POINTER_INVALID(Exposure)

    status = KeWaitForSingleObject(&SensorCtx->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s GetExposure KeWaitForSingleObject fail, (status 0x%x)\n",
            DEVICE_NAME, status);
        goto invalid;
    }

    //get coarse integration time
    status = RegRead(SensorCtx, REG_EXPOSURE, &data, DATA_24BITS);
    if (!NT_SUCCESS(status))
        goto done;

    Exposure->CoarseIntegrationTime = (USHORT)(data >> 4);

    //get fine integration time
    Exposure->FineIntegrationTime = 0;

    //get analog gain
    status = RegRead(SensorCtx, REG_ANA_GAIN, &data, DATA_16BITS);
    if (!NT_SUCCESS(status))
        goto done;

    Exposure->AnalogGain = (USHORT)data;

    //get digital gain

    status = RegRead(SensorCtx, REG_MWB_GAIN_R, &data, DATA_16BITS);
    if (!NT_SUCCESS(status))
        goto done;

    Exposure->DigitalGain = (USHORT)data;
done:
    KeSetEvent(&SensorCtx->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, "%s SampleCmd_GetExposure returns with code: 0x%x\n", DEVICE_NAME, status);
    return status;
}

NTSTATUS
LocalCmd_GetGain(
    PDEVICE_CONTEXT SensorCtx,
    ULONG *Gain
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG Analog;

    CHECK_POINTER_INVALID(SensorCtx)
    CHECK_POINTER_INVALID(Gain)

    status = KeWaitForSingleObject(&SensorCtx->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s GetGain KeWaitForSingleObject fail, (status 0x%x)\n",
            DEVICE_NAME, status);
        goto invalid;
    }

    status = RegRead(SensorCtx, REG_ANA_GAIN, &Analog, DATA_16BITS);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s fail to get gain\n", DEVICE_NAME);
        goto done;
    }

    *Gain = Analog;
done:
    KeSetEvent(&SensorCtx->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, "%s SampleCmd_GetGain (%d) returns with code: 0x%x\n", DEVICE_NAME,
        Gain ? *Gain : 0, status);
    return status;
}

NTSTATUS
Cmd_OtpRead(
    PNVM_CONTEXT pSsNvm,
    UINT16 Addr,
    UINT8* pData,
    INT32 Length
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    UINT16 bank;
    UINT16 start, end;
    UINT16 index;
    UINT8 output[OTP_BANK_SIZE] = { 0xFF };
    UINT16 reserve;

    PDEVICE_CONTEXT SensorCtx = (PDEVICE_CONTEXT)pSsNvm->pDeviceCtx;

    status = KeWaitForSingleObject(&SensorCtx->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SetExposure KeWaitForSingleObject fail, (status 0x%x)\n",
            DEVICE_NAME, status);
        goto invalid;
    }

    //otp read/write can only work when sensor is in power on state
    if (SensorCtx->PowerOnStatus == PWR_DOWN)
    {
        DoTraceMessage(FLAG_ERROR, "%s Failed to Read OTP data (it is power down state)\n", DEVICE_NAME);
        goto done;
    }

    if (Addr + Length > OTP_SIZE)
    {
        DoTraceMessage(FLAG_ERROR, "%s Failed to Read OTP data (size error %d)\n", DEVICE_NAME, Length);
        goto done;
    }

    start = Addr % OTP_BANK_SIZE;
    end = (Addr + Length - 1) % OTP_BANK_SIZE;
    bank = (Addr / OTP_BANK_SIZE);

    // setting
    /* Streaming has to be on */
    status = RegWrite(SensorCtx, SAMPLE_STREAM_MODE, 0x01, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    /* Turn off Dead Pixel Correction */
    status = RegWrite(SensorCtx, SAMPLE_OTP_ISP_CTRL2, 0x00, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    /* Enable partial OTP read mode */
    status = RegWrite(SensorCtx, SAMPLE_OTP_MODE_CTRL, SAMPLE_OTP_MODE_PROGRAM_DISABLE | SAMPLE_OTP_MODE_MANUAL, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    do{
        // end
        DoTraceMessage(FLAG_LOG, "Start read otp data from addr %d:\n", SAMPLE_OTP_START_ADDR + OTP_BANK_SIZE* (bank));
        reserve = (Addr + (UINT16)Length - 1) - (bank) * OTP_BANK_SIZE;
        if (reserve <= OTP_BANK_SIZE)
            end = (Addr + Length - 1) % OTP_BANK_SIZE;
        else
            end = OTP_BANK_SIZE - 1;

        status = RegWrite(SensorCtx, SAMPLE_OTP_START_ADDR_REG, SAMPLE_OTP_START_ADDR + OTP_BANK_SIZE* (bank), DATA_8BITS);
        if (!NT_SUCCESS(status))
            goto done;

        status = RegWrite(SensorCtx, SAMPLE_OTP_END_ADDR_REG, SAMPLE_OTP_START_ADDR + OTP_BANK_SIZE* (bank+1)-1, DATA_8BITS);
        if (!NT_SUCCESS(status))
            goto done;

        status = RegWrite(SensorCtx, SAMPLE_OTP_LOAD_CTRL, SAMPLE_OTP_LOAD_ENABLE, DATA_8BITS);
        if (!NT_SUCCESS(status))
            goto done;

        SleepMSecond(1);

        status = RegReadBuf(&SensorCtx->I2cDev, SAMPLE_OTP_START_ADDR + OTP_BANK_SIZE* (bank), output, OTP_BANK_SIZE);
        if (!NT_SUCCESS(status))
            goto done;
        DoTraceMessage(FLAG_LOG, "%s bank%d:\n", DEVICE_NAME, bank);
        DoTraceMessage(FLAG_LOG, "%s 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x \n",
            DEVICE_NAME, output[0], output[1], output[2], output[3], output[4], output[5], output[6], output[7], output[8],
            output[9], output[10], output[11], output[12], output[13], output[14], output[15]);

        for (index = 0; index < OTP_BANK_SIZE; index++)
        {
            if (index >= start && index <= end)
                *pData++ = output[index];
        }
        start = 0;
        bank = bank + 1;
    } while (reserve > OTP_BANK_SIZE);

    /* Turn on Dead Pixel Correction */
    status = RegWrite(SensorCtx, SAMPLE_OTP_ISP_CTRL2, SAMPLE_OTP_DPC_ENABLE, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;

    /* Streaming has to be off */
    status = RegWrite(SensorCtx, SAMPLE_STREAM_MODE, 0x00, DATA_8BITS);
    if (!NT_SUCCESS(status))
        goto done;



done:
    KeSetEvent(&SensorCtx->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, "%s SampleCmd_OTPRead addr = 0x%x, size = (%d),  returns with code: 0x%x\n", DEVICE_NAME,
        Addr, Length, status);
    return status;
}



NTSTATUS
Cmd_OtpWrite(
    PNVM_CONTEXT pSsNvm,
    UINT16 Addr,
    UINT8* pData,
    INT32 Length
    )
{
    UNREFERENCED_PARAMETER(pSsNvm);
    UNREFERENCED_PARAMETER(Addr);
    UNREFERENCED_PARAMETER(pData);
    UNREFERENCED_PARAMETER(Length);

    return STATUS_SUCCESS;
}

#if defined(OTM_SENSOR_CONTROL_LOGIC)
NTSTATUS
SensorPower(PDEVICE_CONTEXT SensorCtx, BOOLEAN OnOff)
{
    DoTraceMessage(FLAG_ERROR, "%s enter SensorPower", DEVICE_NAME);

    NTSTATUS status = STATUS_SUCCESS;
    if (SensorCtx->SKU.crd == SKU_CRD_D ||
        SensorCtx->SKU.crd == SKU_CRD_G ||
        SensorCtx->SKU.crd == SKU_CRD_G2 ||
        SensorCtx->SKU.crd == SKU_CRG_VALUE)
    {
        if (SensorCtx->ControlLogicInterface)
        {
            if (OnOff)
            {
                status = SensorCtx->ControlLogicInterface->SensorOn(SensorCtx->ControlLogicInterface->InterfaceHeader.Context, &SensorCtx->SensorConf);
            }
            else
            {
                status = SensorCtx->ControlLogicInterface->SensorOff(SensorCtx->ControlLogicInterface->InterfaceHeader.Context, &SensorCtx->SensorConf);
            }

            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s power on/off fail, callback function failure! (status 0x%x)\n", DEVICE_NAME, status);
            }
        }
#if defined(OTM_SENSOR_CONTROL_LOGIC)
        else
        {
            // work around [11/16/2015 shixx]
            // camera module power on/ff ctrl
            if (OnOff == TRUE)
            {

                status = SsPowerGpio(SensorCtx, TRUE);
                if (!NT_SUCCESS(status))
                {
                    DoTraceMessage(FLAG_ERROR, "%s GpioPinControl On Fail!\n", DEVICE_NAME);
                }
            }
            else
            {
                // GPIO off
                status = SsPowerGpio(SensorCtx, FALSE);
                if (!NT_SUCCESS(status))
                {
                    DoTraceMessage(FLAG_ERROR, "%s Failed to power off sensor\n", DEVICE_NAME);
                }
            }

            DoTraceMessage(FLAG_LOG, "%s power on/off by GPIO\n", DEVICE_NAME);
        }
#endif
    }
    else
    {
        status = STATUS_UNSUCCESSFUL;
        DoTraceMessage(FLAG_ERROR, "%s power on/off fail, SKU is not right! (status 0x%x)\n", DEVICE_NAME, status);
    }

    return status;
}
#endif


char* ReturnSensorNameToLib(void) {return SENSOR_NAME;}            
ULONG ReturnPoolTagToLib(void) {return POOL_TAG;}                    
ULONG ReturnOtpValidSizeToLib(void) {return OTP_VALID_SIZE;}        
