/*************************************************************************************************
// INTEL CONFIDENTIAL Copyright 2011-2018 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 "SensorTrace.h"
#include "SsCommon.h"
#include "platf_info.h"
#include "SsSensor.h"
#include "SsSensor.tmh"
#include "Sensor.h"


#define CAMERA_DRIVER_REGISTRY_PATH L"\\REGISTRY\\MACHINE\\SYSTEM\\ControlSet001\\Services\\camera"
#define MAX_BUF (32+2)

static void write_i2c_type(UINT8* buf, ULONG data, DATA_TOKE_TYPE type)
{
    if (!buf)
    {
        return;
    }
    
    switch (type)
    {
    case DATA_8BITS:
        buf[0] = (UINT8)data;
        break;
    case DATA_16BITS:
        buf[0] = (UINT8)(data >> 8);
        buf[1] = (UINT8)(data >> 0);
        break;
    case DATA_24BITS:
        buf[0] = (UINT8)(data >> 16);
        buf[1] = (UINT8)(data >> 8);
        buf[2] = (UINT8)(data >> 0);
        break;
    case DATA_32BITS:
        buf[0] = (UINT8)(data >> 24);
        buf[1] = (UINT8)(data >> 16);
        buf[2] = (UINT8)(data >> 8);
        buf[3] = (UINT8)(data >> 0);
        break;
    default:
        return;
    }
}

static BOOLEAN IsCNL(UCHAR* pInformation)
{
    CHAR* pInfo = (CHAR*)pInformation;

    if (pInfo[0] == 'C' && pInfo[1] == 'N' && pInfo[2] == 'L')
        return TRUE;

    return FALSE;
}


NTSTATUS RegWrite(
    PDEVICE_CONTEXT SensorCtx,
    UINT16 Addr,
    ULONG Data,
    DATA_TOKE_TYPE DataBits
    )
{
    NTSTATUS                status = STATUS_SUCCESS;

    CHECK_POINTER(SensorCtx)
    if (DataBits == DATA_TOKE_DELAY)
    {
        SleepMSecond(Data);  
        return STATUS_SUCCESS;
    }

    status = i2cWrite(&SensorCtx->I2cDev, Addr, Data, DataBits);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s write 0x%x <- 0x%x try again!! (0x%x)\n", DEVICE_NAME, Addr, Data, status);
        SleepMSecond(2);
        status = i2cWrite(&SensorCtx->I2cDev, Addr, Data, DataBits);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s write 0x%x <- 0x%x fail!! (0x%x)\n", DEVICE_NAME, Addr, Data, status);
            goto done;
        }
    }
    if (DataBits == DATA_8BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%04x -> 0x%02x (0x%x)\n", DEVICE_NAME, Addr, Data, status);
    else if (DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%04x -> 0x%04x (0x%x)\n", DEVICE_NAME, Addr, Data, status);
    else if (DataBits == DATA_24BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%04x -> 0x%06x (0x%x)\n", DEVICE_NAME, Addr, Data, status);
done:
    return status;
}

NTSTATUS RegWriteExt(
    PDEVICE_CONTEXT SensorCtx,
    ULONG Addr,
    ULONG Data,
    DATA_TOKE_TYPE DataBits,
    ADDR_TOKE_TYPE AddrBits,
    ULONG I2CIndex
    )
{
    NTSTATUS  status = STATUS_SUCCESS;

    CHECK_POINTER(SensorCtx)
    if (DataBits == DATA_TOKE_DELAY)
    {
        SleepMSecond(Data);
        return STATUS_SUCCESS;
    }

    status = i2cWriteExt(&SensorCtx->I2cDev, Addr, Data, DataBits, AddrBits, I2CIndex);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s write 0x%x <- 0x%x try again!! (0x%x)\n", DEVICE_NAME, Addr, Data, status);
        SleepMSecond(2);
        status = i2cWriteExt(&SensorCtx->I2cDev, Addr, Data, DataBits, AddrBits, I2CIndex);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s write 0x%x <- 0x%x fail!! (0x%x)\n", DEVICE_NAME, Addr, Data, status);
            goto done;
        }
    }
    if (AddrBits == ADDR_8BITS && DataBits == DATA_8BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%02x -> 0x%02x (0x%x)\n", DEVICE_NAME, Addr, Data, status);

    if (AddrBits == ADDR_16BITS && DataBits == DATA_8BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%04x -> 0x%02x (0x%x)\n", DEVICE_NAME, Addr, Data, status);

    if (AddrBits == ADDR_16BITS && DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%04x -> 0x%04x (0x%x)\n", DEVICE_NAME, Addr, Data, status);

    if (AddrBits == ADDR_16BITS && DataBits == DATA_32BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%04x -> 0x%08x (0x%x)\n", DEVICE_NAME, Addr, Data, status);

    if (AddrBits == ADDR_32BITS && DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%08x -> 0x%04x (0x%x)\n", DEVICE_NAME, Addr, Data, status);
done:
    return status;
}

NTSTATUS RegRead(
    PDEVICE_CONTEXT SensorCtx,
    UINT16 Addr,
    ULONG *Data,
    DATA_TOKE_TYPE DataBits
    )
{
    NTSTATUS    status = STATUS_SUCCESS;

    if (!SensorCtx || !Data)
    {
        return STATUS_INVALID_PARAMETER;
    }

    status = i2cRead(&SensorCtx->I2cDev, Addr, Data, DataBits);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s read reg 0x%x try again!! (0x%x)\n", DEVICE_NAME, Addr, status);
        SleepMSecond(2);
        status = i2cRead(&SensorCtx->I2cDev, Addr, Data, DataBits);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s read reg 0x%x fail!! (0x%x)\n", DEVICE_NAME, Addr, status);
            goto done;
        }
    }
    if (DataBits == DATA_8BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%02x (0x%x)\n", DEVICE_NAME, Addr, *Data, status);
    else if (DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%04x (0x%x)\n", DEVICE_NAME, Addr, *Data, status);
    else if (DataBits == DATA_24BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%06x (0x%x)\n", DEVICE_NAME, Addr, *Data, status);
done:
    return status;
}

NTSTATUS RegReadExt(
    PDEVICE_CONTEXT SensorCtx,
    ULONG Addr,
    ULONG *Data,
    DATA_TOKE_TYPE DataBits,
    ADDR_TOKE_TYPE AddrBits,
    ULONG I2CIndex
    )
{
    NTSTATUS    status = STATUS_SUCCESS;

    if (!SensorCtx || !Data)
    {
        return STATUS_INVALID_PARAMETER;
    }

    status = i2cReadExt(&SensorCtx->I2cDev, Addr, Data, DataBits, AddrBits, I2CIndex);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s read reg 0x%x try again!! (0x%x)\n", DEVICE_NAME, Addr, status);
        SleepMSecond(2);
        status = i2cReadExt(&SensorCtx->I2cDev, Addr, Data, DataBits, AddrBits, I2CIndex);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s read reg 0x%x fail!! (0x%x)\n", DEVICE_NAME, Addr, status);
            goto done;
        }
    }

    if (AddrBits == ADDR_8BITS && DataBits == DATA_8BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%02x -> 0x%02x (0x%x)\n", DEVICE_NAME, Addr, *Data, status);

    if (AddrBits == ADDR_16BITS && DataBits == DATA_8BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%02x (0x%x)\n", DEVICE_NAME, Addr, *Data, status);

    if (AddrBits == ADDR_16BITS && DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%04x (0x%x)\n", DEVICE_NAME, Addr, *Data, status);

    if (AddrBits == ADDR_16BITS && DataBits == DATA_32BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%08x (0x%x)\n", DEVICE_NAME, Addr, *Data, status);

    if (AddrBits == ADDR_32BITS && DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%08x -> 0x%04x (0x%x)\n", DEVICE_NAME, Addr, *Data, status);
done:
    return status;
}

NTSTATUS RegWriteBufSlow(
    PDEVICE_CONTEXT pDevExt,
    const UINT8* Data,
    ULONG Length
    )
{
    NTSTATUS   status = STATUS_SUCCESS;
    UINT8   buffer[130];
    INT32   index, intcount, resize;
    UINT16  baseaddr, addr;

    if (pDevExt->I2cDev.I2c[I2C_GENERAL].AddrBits != ADDR_16BITS)
        goto done;

    baseaddr = ((Data[0]<<8) | Data[1]);
    intcount = (Length - ADDR_16BITS) / 128;
    resize = (Length - ADDR_16BITS) % 128;

    for (index=0; index<intcount; index++)
    {
        addr = (UINT16)(baseaddr + 128*index);
        buffer[0] = (UINT8)(addr >> 8);
        buffer[1] = (UINT8)(addr);
        memcpy_s(buffer+2, 128, Data+2+128*index, 128);
        RegWriteBuf(&pDevExt->I2cDev, buffer, 128+2);
    }

    if (resize!=0)
    {
        addr = (UINT16)(baseaddr + 128*index);
        memcpy_s(buffer+2, resize, Data+2+128*index, resize);
		RegWriteBuf(&pDevExt->I2cDev, buffer, resize + 2);
    }
    
done:
    return status;
}

NTSTATUS RegWriteQueueSlow(
    PDEVICE_CONTEXT SensorCtx,
    const REGS_SETTING *regs
    )
{
    const REGS_SETTING *p = regs;
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(SensorCtx)
    CHECK_POINTER(regs)

    while(p->Type != DATA_TOKE_TERM)
    {
        if (p->Type >= DATA_8BITS && p->Type <= DATA_32BITS)
        {
            status = RegWrite(SensorCtx, p->Reg, p->Data, p->Type);
            if (!NT_SUCCESS(status))
                goto done;
        }
        if (p->Type == DATA_TOKE_DELAY)
            SleepMSecond(p->Data);

        p++;
    };
done:
    return status;
}


NTSTATUS RegWriteQueue(
    PDEVICE_CONTEXT SensorCtx,
    const REGS_SETTING *regs
    )
{
    const REGS_SETTING *p = regs;
    NTSTATUS status = STATUS_SUCCESS;
    UINT8 wbuf[MAX_BUF] = {0};
    UINT8 rbuf[32] = {0};
    INT32 index = 0;
    ADDR_TOKE_TYPE  AddrBits = SensorCtx->I2cDev.I2c[I2C_GENERAL].AddrBits;

    CHECK_POINTER(SensorCtx)
    CHECK_POINTER(regs)

    while(p->Type != DATA_TOKE_TERM)
    {
        if (p->Type >= DATA_8BITS && p->Type <= DATA_32BITS)
        {
            index = 0;
            if (AddrBits == ADDR_8BITS)
            {
                wbuf[index++] = (UINT8)p->Reg;
            }
            else
            {
                wbuf[index++] = (UINT8)(p->Reg >> 8);
                wbuf[index++] = (UINT8)p->Reg;
            }
            if (p->Type == DATA_8BITS)
                DoTraceMessage(FLAG_LOG, "%s addr 0x%04x, wbuf[%d]=0x%02x", DEVICE_NAME, p->Reg, 0, p->Data);
            else if (p->Type == DATA_16BITS)
                DoTraceMessage(FLAG_LOG, "%s addr 0x%04x, wbuf[%d]=0x%04x", DEVICE_NAME, p->Reg, 0, p->Data);

            do
            {
                write_i2c_type(&wbuf[index], p->Data, p->Type);
                index += p->Type;

                if((p+1)->Type < DATA_8BITS || (p+1)->Type > DATA_32BITS )
                    break;

                if(index + (p+1)->Type > MAX_BUF)
                    break;

                if(p->Reg + p->Type != (p+1)->Reg)
                    break;

                p ++;
            }while(p->Type != DATA_TOKE_TERM);
            status = RegWriteBuf(&SensorCtx->I2cDev, wbuf, index);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to write reg 0x%x\n", DEVICE_NAME, status);
                goto done;
            }
            
            if (SensorCtx->Key_IsRegCheck)
            {
                int i;
                unsigned short int adr;
                for (i = 1; i < index - AddrBits; i ++)
                    DoTraceMessage(FLAG_LOG, "%s          , wbuf[%d]=0x%x", DEVICE_NAME, i, wbuf[i+AddrBits]);

                adr = (AddrBits == ADDR_8BITS) ? wbuf[0] : (wbuf[0] << 8) | wbuf[1];
                
                status = RegReadBuf(&SensorCtx->I2cDev, adr, rbuf, index - AddrBits);
                if (!NT_SUCCESS(status))
                    DoTraceMessage(FLAG_ERROR, "%s Failed to write reg 0x%x\n", DEVICE_NAME, status);
                
                DoTraceMessage(FLAG_LOG, "%s addr 0x%x, rbuf[%d]=0x%x", DEVICE_NAME, adr, 0, rbuf[0]);
                for (i = 1; i < index - AddrBits; i ++)
                    DoTraceMessage(FLAG_LOG, "%s          , rbuf[%d]=0x%x", DEVICE_NAME, i, rbuf[i]);
            }
        }

        if (p->Type == DATA_TOKE_DELAY)
            SleepMSecond(p->Data);

        p++;
    };
done:
    return status;
}

NTSTATUS RegWriteQueueExt(
    PDEVICE_CONTEXT SensorCtx,
    const REGS_SETTING_EXT *regs,
    ULONG I2CIndex
    )
{
    const REGS_SETTING_EXT *p = regs;
    NTSTATUS status = STATUS_SUCCESS;
    UINT8 wbuf[MAX_BUF] = {0};
    INT32 index = 0;

    CHECK_POINTER(SensorCtx)
    CHECK_POINTER(regs)

    while (p->Type != DATA_TOKE_TERM)
    {
        if (p->Type >= DATA_8BITS && p->Type <= DATA_32BITS)
        {
            index = 0;
            if (p->AddrType == ADDR_8BITS)
            {
                wbuf[index++] = (UINT8)p->Reg;
            }
            else if (p->AddrType == ADDR_16BITS)
            {
                wbuf[index++] = (UINT8)(p->Reg >> 8);
                wbuf[index++] = (UINT8)p->Reg;
            }
            else if (p->AddrType == ADDR_24BITS)
            {
                wbuf[index++] = (UINT8)(p->Reg >> 16);
                wbuf[index++] = (UINT8)(p->Reg >> 8);
                wbuf[index++] = (UINT8)p->Reg;
            }
            else if (p->AddrType == ADDR_32BITS)
            {
                wbuf[index++] = (UINT8)(p->Reg >> 24);
                wbuf[index++] = (UINT8)(p->Reg >> 16);
                wbuf[index++] = (UINT8)(p->Reg >> 8);
                wbuf[index++] = (UINT8)p->Reg;
            }
            if (p->Type == DATA_8BITS)
                DoTraceMessage(FLAG_LOG, "%s addr 0x%04x, wbuf[%d]=0x%02x", DEVICE_NAME, p->Reg, 0, p->Data);
            else if (p->Type == DATA_16BITS)
                DoTraceMessage(FLAG_LOG, "%s addr 0x%04x, wbuf[%d]=0x%04x", DEVICE_NAME, p->Reg, 0, p->Data);
            else if (p->Type == DATA_24BITS)
                DoTraceMessage(FLAG_LOG, "%s addr 0x%04x, wbuf[%d]=0x%06x", DEVICE_NAME, p->Reg, 0, p->Data);
            else if (p->Type == DATA_32BITS)
                DoTraceMessage(FLAG_LOG, "%s addr 0x%04x, wbuf[%d]=0x%08x", DEVICE_NAME, p->Reg, 0, p->Data);

            do
            {
                write_i2c_type(&wbuf[index], p->Data, p->Type);
                index += p->Type;

                if ((p + 1)->Type < DATA_8BITS || (p + 1)->Type > DATA_32BITS)
                    break;

                if (index + (p + 1)->Type > MAX_BUF)
                    break;

                if (p->Reg + p->Type != (p + 1)->Reg)
                    break;

                p++;
            } while (p->Type != DATA_TOKE_TERM);
            status = RegWriteBufExt(&SensorCtx->I2cDev, wbuf, index, I2CIndex);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to write reg 0x%x\n", DEVICE_NAME, status);
                goto done;
            }
        }

        if (p->Type == DATA_TOKE_DELAY)
            SleepMSecond(p->Data);

        p++;
    };
done:
    return status;
}

#ifdef SKY_CAM
NTSTATUS SsIvcamGetCalibrationData(
    PULONG pHandle,
    PUINT8 pBuffer
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    PDEVICE_CONTEXT pDevExt = (PDEVICE_CONTEXT)pHandle;

    if (pDevExt->IvcamSCLInterface)
    {
        status = pDevExt->IvcamSCLInterface->GetCalibrationData(pDevExt->IvcamSCLInterface->Interface.Context,
                                                                0, 512, pBuffer);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s IVCAM SCL interface GetCalibrationData error 0x%x\n", DEVICE_NAME, status);
            goto done;
        }
    }
    else
    {
        status = STATUS_INVALID_HANDLE;
        DoTraceMessage(FLAG_ERROR, "%s IVCAM SCL interface have not initialize 0x%x\n", DEVICE_NAME, status);
    }

done:
    return status;
}
#endif


static SNSR_RESOLUTION ExportPreview[RES_IMAGE_MAX];
static SNSR_RESOLUTION ExportStill[RES_IMAGE_MAX];
static SNSR_RESOLUTION ExportVideo[RES_VIDEO_MAX];
static SNSR_RESOLUTION ExportRaw[RES_BAYER_MAX];
SNSR_RESOLUTION_SET ExportResolutionSet[maxMode];


SENSOR_RESOLUTION* Sensor_Res[maxMode] = 
{
    sensor_res_still,
    sensor_res_video,
    sensor_res_preview,
    sensor_res_raw,
};

static void InitTableUnit(
    PDEVICE_CONTEXT deviceCtx,
    SENSOR_RESOLUTION* pRes,
    SNSR_RESOLUTION* pTable,
    INT32 MaxNum
    )
{
    INT32 index;

    if (!deviceCtx || !pRes || !pTable)
    {
        return;
    }
    
    UNREFERENCED_PARAMETER(deviceCtx);

    for (index = 0; index < MaxNum; index ++)
    {
        pTable[index].Width = pRes[index].Width;
        pTable[index].Height = pRes[index].Height;
        pTable[index].FPS = pRes[index].FPS;
        pTable[index].BayerOrder = (deviceCtx->Orientation == DEGREE_180) ? pRes[index].BayerOrder180Degree : pRes[index].BayerOrder;
        pTable[index].IsFullFov = pRes[index].IsFullFov;
        pTable[index].CropX = 1000;
        pTable[index].CropY = 1000;
        DoTraceMessage(FLAG_LOG, "%s {%d, %d, %d, %d, %d, %d, %d}\n", DEVICE_NAME, 
            pTable[index].Width, pTable[index].Height, 
            pTable[index].CropX, pTable[index].CropY,
            pTable[index].FPS, pTable[index].BayerOrder,
            pTable[index].IsFullFov);
    }
}

NTSTATUS SsInitExportResTable(PDEVICE_CONTEXT deviceCtx)
{
    NTSTATUS status = STATUS_SUCCESS;
    INT32 ResNum, mode, i;
    INT32 MaxNum[maxMode] = {RES_IMAGE_MAX, RES_VIDEO_MAX, RES_IMAGE_MAX, RES_BAYER_MAX};
    const char* PrintString[maxMode] = {"Still", "Video", "Preview", "Raw"};
    SNSR_RESOLUTION* ResExport[maxMode] = {ExportStill, ExportVideo, ExportPreview, ExportRaw};

    if (!deviceCtx)
    {
        return STATUS_INVALID_PARAMETER;
    }

    for (mode = 0; mode < maxMode; mode ++)
    {
        for (i = 0; i <= RES_BAYER_MAX; i ++)
        {
#ifdef SKY_CAM
            if (Sensor_Res[mode][i].Width== 0) 
                break;
            if(Sensor_Res[mode][i].FPS == 15) //only expose 15fps for SKY1 project
            {
                if (_stricmp((CHAR*)deviceCtx->DsdtSettings.ModuleName, "MSHW0070") != 0 
                    && _stricmp((CHAR*)deviceCtx->DsdtSettings.ModuleName, "MSHW0071") != 0 
                    && _stricmp((CHAR*)deviceCtx->DsdtSettings.ModuleName, "MSHW0073") != 0 //MSChange - add OV9734
                    && _stricmp((CHAR*)deviceCtx->DsdtSettings.ModuleName, "MSHW0120") != 0
                    && _stricmp((CHAR*)deviceCtx->DsdtSettings.ModuleName, "MSHW0121") != 0
                    && _stricmp((CHAR*)deviceCtx->DsdtSettings.ModuleName, "MSHW0140") != 0
                    && _stricmp((CHAR*)deviceCtx->DsdtSettings.ModuleName, "MSHW0141") != 0
                    && _stricmp((CHAR*)deviceCtx->DsdtSettings.ModuleName, "MSHW0150") != 0
                    && _stricmp((CHAR*)deviceCtx->DsdtSettings.ModuleName, "MSHW0151") != 0)
                    break;
            }
#else
            if (Sensor_Res[mode][i].regs == NULL)
                break;
#endif
        }
        ResNum = i;
        if (ResNum > MaxNum[mode])
        {
            DoTraceMessage(FLAG_LOG, "%s SsInitExportResTable Fail, %s mode count %d overrun\n", DEVICE_NAME, PrintString[mode], ResNum);
            return STATUS_UNSUCCESSFUL;
        }
        
        ExportResolutionSet[mode].Count = ResNum;
        ExportResolutionSet[mode].Resolutions = ResExport[mode];
        DoTraceMessage(FLAG_LOG, "%s %s %d Resolutions\n", DEVICE_NAME, PrintString[mode], ResNum);
        InitTableUnit(deviceCtx, Sensor_Res[mode], ExportResolutionSet[mode].Resolutions, ResNum);
    }
    return status;
}

static int distance(SENSOR_RESOLUTION *res, ULONG w, ULONG h)
{
    int ret;

    if (!res)
        return -1;
    
    if ((res->Width < w) || (res->Height < h))
        return -1;

    ret = ((res->Width - w) + (res->Height - h));
    return ret;
}

static int FindResIndex(SNSR_MODE mode, INT32 w, INT32 h, INT32 frameRate)
{
    int i;
    int idx = -1;
    int dist;
    int min_dist = 0xffff;
    int N_RES = 0;
    SENSOR_RESOLUTION *tmp_res = NULL;

    N_RES = ExportResolutionSet[mode].Count;

    for (i = 0; i < N_RES; i++)
    {
        tmp_res = &(Sensor_Res[mode][i]);
        dist = distance(tmp_res, (ULONG)w, (ULONG)h);
        if (dist == -1)
            continue;
        
        // skip check for minus frame rate (e.g. fifosensor, slevirtualsensor)
        if (frameRate > 0 && frameRate != tmp_res->FPS)
            continue;

        if (dist < min_dist) 
        {
            min_dist = dist;
            idx = i;
        }
    }

    if (idx == -1)
        return -1;

    return idx;
}

#ifdef AE_EXACT_CONFIG

UINT32 expIndex(LONG frame)
{
    return abs((frame + EXP_TABLE_NUM)%EXP_TABLE_NUM);
}

/// Public API
NTSTATUS
aeCmd_SetExposure(
    PDEVICE_CONTEXT pDevExt,
    SNSR_EXPOSURE ExposureTime
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    INT32 index;

    CHECK_POINTER_INVALID(pDevExt)


    if (pDevExt->N_Frame < -3)
    {
        DoTraceMessage(FLAG_ERROR, "N_Frame error: %d", pDevExt->N_Frame);
        goto done;
    }

    // Start Mutex
    status = KeWaitForSingleObject(&pDevExt->ExpEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s aeCmd_SetExposure KeWaitForSingleObject fail, (status 0x%x)\n",
            DEVICE_NAME, status);
        goto invalid;
    }

    index = expIndex(pDevExt->N_Frame + 3);

    pDevExt->ExpTab[index].Exp.CoarseIntegrationTime = ExposureTime.CoarseIntegrationTime;
    pDevExt->ExpTab[index].Exp.FineIntegrationTime = ExposureTime.FineIntegrationTime;
    pDevExt->ExpTab[index].Exp.FrameLengthLines = ExposureTime.FrameLengthLines;
    pDevExt->ExpTab[index].Exp.LineLengthPixels = ExposureTime.LineLengthPixels;
    pDevExt->ExpTab[index].Exp.AnalogGain = ExposureTime.AnalogGain;
    pDevExt->ExpTab[index].Exp.DigitalGain = ExposureTime.DigitalGain;
    pDevExt->ExpTab[index].ExpStatus = EXPSTS_UPDATED;
    pDevExt->ExpTab[index].GainStatus = EXPSTS_UPDATED;
    pDevExt->ExpTab[index].DGainStatus = EXPSTS_UPDATED;
    pDevExt->IsThereNewExp = TRUE;

    // if the before streaming, set exposure time directly
    if (pDevExt->Streaming == FALSE)
    {
         status = LocalCmd_SetExposure(pDevExt, ExposureTime);
    }
invalid:
    // End Mutex
    KeSetEvent(&pDevExt->ExpEvent, IO_NO_INCREMENT, FALSE);
done:    
    DoTraceMessage(FLAG_LOG, 
        "%s aeCmd_SetExposure: Coarse (%d), AnalogGain (%d) DigitalGain (%d) During Frame %d,  For Frame %d\n", DEVICE_NAME, 
        ExposureTime.CoarseIntegrationTime, ExposureTime.AnalogGain, ExposureTime.DigitalGain, pDevExt->N_exp_id+1, pDevExt->N_exp_id+4);
    return status;
}

_Function_class_ (IO_WORKITEM_ROUTINE) static VOID WorkerExposure(
        __in PDEVICE_OBJECT  DeviceObject,
        __in_opt PVOID  Context
        )
{
    PAGED_CODE();
    UNREFERENCED_PARAMETER(DeviceObject);
    UNREFERENCED_PARAMETER(Context);

    PDEVICE_CONTEXT deviceCtx = (PDEVICE_CONTEXT)Context;
    DoTraceMessage(FLAG_LOG, "Worker TimerExposure\n");
    expSetExposure(deviceCtx, deviceCtx->ExpFetch.Exp, 
        deviceCtx->ExpFetch.ExpStatus, 
        deviceCtx->ExpFetch.GainStatus,
        deviceCtx->ExpFetch.DGainStatus);
}

_Function_class_ (KDEFERRED_ROUTINE) void DPCExpFunc(
    IN PKDPC Dpc,
    IN PVOID DeferredContext,
    IN PVOID SystemArg1,
    IN PVOID SystemArg2
    )
{
    UNREFERENCED_PARAMETER(Dpc);
    UNREFERENCED_PARAMETER(SystemArg1);
    UNREFERENCED_PARAMETER(SystemArg2);

    PDEVICE_CONTEXT deviceCtx = (PDEVICE_CONTEXT)DeferredContext;

    LARGE_INTEGER remTime;
    //LARGE_INTEGER curTime;
    LONG durMs, curMs;

    if (!deviceCtx)
        return;

    //GetTickCount(&curTime);
    //curMs = (LONG)(curTime.QuadPart);
    curMs = (LONG)QuerySystemTime();
    durMs = curMs - deviceCtx->preTimeExp;
    deviceCtx->preTimeExp = curMs;
    deviceCtx->remainTimeExp = deviceCtx->remainTimeExp - durMs;

    DoTraceMessage(FLAG_LOG, "DPC TimerExposure, cur=%dms, remain=%dms \n", deviceCtx->preTimeExp, deviceCtx->remainTimeExp);

    if (deviceCtx->remainTimeExp > 0 && deviceCtx->remainTimeExp < 30)
    {
        remTime.QuadPart = deviceCtx->remainTimeExp * (-10000);
        KeSetTimer(&(deviceCtx->TimerExposure), remTime, &(deviceCtx->DPCExposure));
    }
    else
    {
        IoQueueWorkItem(
            deviceCtx->WorkItemExposure,
            WorkerExposure,
            CriticalWorkQueue,
            DeferredContext
            );
    }
}

void expInit(
    PDEVICE_CONTEXT deviceCtx
    )
{
    deviceCtx->SensorInterface.SetExposure              = aeCmd_SetExposure;
    // risk to overun prevous special SexExtFeature for certain sensor driver
    deviceCtx->SensorInterface.SetExtFeature            = Cmd_SetExtFeature;

    // exposure exactly control
    deviceCtx->N_Frame = -3;
    deviceCtx->N_exp_id = 0;
    deviceCtx->IsThereNewExp = FALSE;
    {
        int i;
        for (i = 0;  i < EXP_TABLE_NUM; i ++)
        {
            deviceCtx->ExpTab[i].ExpStatus = EXPSTS_INVALID;
            deviceCtx->ExpTab[i].GainStatus = EXPSTS_INVALID;
            deviceCtx->ExpTab[i].DGainStatus = EXPSTS_INVALID;
        }
    }
    deviceCtx->ExpFetch.ExpStatus = EXPSTS_INVALID;
    deviceCtx->ExpFetch.GainStatus = EXPSTS_INVALID;
    deviceCtx->ExpFetch.DGainStatus = EXPSTS_INVALID;

    deviceCtx->ExpEffectDelay = ExpEffectDelayFrames[0];
    deviceCtx->AGainEffectDelay = ExpEffectDelayFrames[1];
    deviceCtx->DGainEffectDelay = ExpEffectDelayFrames[2];

    KeInitializeEvent (&deviceCtx->ExpEvent, SynchronizationEvent, TRUE);

    deviceCtx->DeviceObject = WdfDeviceWdmGetPhysicalDevice(deviceCtx->DeviceHandle);

    deviceCtx->WorkItemExposure = IoAllocateWorkItem(deviceCtx->DeviceObject);
    KeInitializeDpc(&(deviceCtx->DPCExposure), DPCExpFunc, deviceCtx);
    KeInitializeTimer(&(deviceCtx->TimerExposure));
}
#endif


NTSTATUS SsPowerGpio(
    PDEVICE_CONTEXT pDevExt,
    BOOLEAN PowerOn
    )
{
    if (!pDevExt)
    {
        return STATUS_INVALID_PARAMETER;
    }
    
    NTSTATUS status = STATUS_SUCCESS;
    PGPIO_CONTEXT pGpioDev = &(pDevExt->GpioDev);
    
    BOOLEAN IsRstPulse = pGpioDev->Gpio[GPIO_PWDN].Flag ? TRUE : FALSE;

    // sensor must take care of S3/S4
    POWER_ACTION sysPowerAction = (POWER_ACTION)(pDevExt->SysPowerAction);

    DoTraceMessage(FLAG_LOG, "%s receive poweraction %d", __FUNCTION__, sysPowerAction);

    //WA for H1504108285. Resume from S3, GPIO will not write pin
    if (sysPowerAction == PowerActionSleep ||
        sysPowerAction == PowerActionHibernate)
    {
        SsGpioPinsUninit(pGpioDev);
    }

    if (PowerOn)
    {
        // Standby
        if (pGpioDev->Gpio[GPIO_STANDBY].Flag)
        {
            status = SsGpioPinSet(pGpioDev, GPIO_STANDBY, 1 - pGpioDev->Gpio[GPIO_STANDBY].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set standby pin active 0x%x\n", DEVICE_NAME, status);
                return status;
            }
            SleepMSecond(DELAY_PWDN);
        }
    
        // POWER_0 for 1.8V
        if (pGpioDev->Gpio[GPIO_POWER0].Flag)
        {
            status = SsGpioPinSet(pGpioDev, GPIO_POWER0, pGpioDev->Gpio[GPIO_POWER0].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set Power 0 pin active 0x%x\n", DEVICE_NAME, status);
                return status;
            }
            SleepMSecond(DELAY_PWDN);
        }

        // POWER_1 for 1.2V
        if (pGpioDev->Gpio[GPIO_POWER1].Flag)
        {
            status = SsGpioPinSet(pGpioDev, GPIO_POWER1, pGpioDev->Gpio[GPIO_POWER1].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set Power 1 pin active 0x%x\n", DEVICE_NAME, status);
                return status;
            }
            SleepMSecond(DELAY_PWDN);
        }

        // Standby
        if (pGpioDev->Gpio[GPIO_STANDBY].Flag)
        {
            status = SsGpioPinSet(pGpioDev, GPIO_STANDBY, pGpioDev->Gpio[GPIO_STANDBY].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set standby pin active 0x%x\n", DEVICE_NAME, status);
                return status;
            }
            SleepMSecond(DELAY_PWDN);
        }

        // PWDN_N
        if (pGpioDev->Gpio[GPIO_PWDN].Flag)
        {
            status = SsGpioPinSet(pGpioDev, GPIO_PWDN, pGpioDev->Gpio[GPIO_PWDN].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set PWDN pin active 0x%x\n", DEVICE_NAME, status);
                return status;
            }
            SleepMSecond(DELAY_PWDN);
        }

        // RESET
        if (pGpioDev->Gpio[GPIO_RESET].Flag)
        {
            if (IsRstPulse)
            {
                // Real Reset
                status = SsGpioPinSet(pGpioDev, GPIO_RESET, 1 - pGpioDev->Gpio[GPIO_RESET].ActiveLevel);
                if (!NT_SUCCESS(status))
                {
                    DoTraceMessage(FLAG_ERROR, "%s Failed to set RESET pin 0x%x\n", DEVICE_NAME, status);
                    return status;
                }
                // RESET pulse width should be greater than or equal to 1ms.
                SleepMSecond(DELAY_RST_PULSE); 
            }
            
            //  pull work
            status = SsGpioPinSet(pGpioDev, GPIO_RESET, pGpioDev->Gpio[GPIO_RESET].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set PWDN pin active 0x%x\n", DEVICE_NAME, status);
                return status;
            }
            // DELAY_RST ms after RESETB goes high, host can access sensor's SCCB
            SleepMSecond(DELAY_RST);
        }
    }
    else
    {
        // RESET
        if (pGpioDev->Gpio[GPIO_RESET].Flag)
        {
            // Real Reset
            status = SsGpioPinSet(pGpioDev, GPIO_RESET, 1 - pGpioDev->Gpio[GPIO_RESET].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set RESET pin 0x%x\n", DEVICE_NAME, status);
                return status;
            }
        }

        // PWDN_N
        if (pGpioDev->Gpio[GPIO_PWDN].Flag)
        {
            status = SsGpioPinSet(pGpioDev, GPIO_PWDN, 1 - pGpioDev->Gpio[GPIO_PWDN].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set PWDN pin inactive 0x%x\n", DEVICE_NAME, status);
                return status;
            }
        }

        // POWER_1 for 1.2V
        if (pGpioDev->Gpio[GPIO_POWER1].Flag)
        {
            status = SsGpioPinSet(pGpioDev, GPIO_POWER1, 1 - pGpioDev->Gpio[GPIO_POWER1].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set Power 1 pin active 0x%x\n", DEVICE_NAME, status);
                return status;
            }
            SleepMSecond(DELAY_PWDN);
        }

        // POWER_0 for 1.8V
        if (pGpioDev->Gpio[GPIO_POWER0].Flag)
        {
            status = SsGpioPinSet(pGpioDev, GPIO_POWER0, 1 - pGpioDev->Gpio[GPIO_POWER0].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Failed to set Power 0 pin active 0x%x\n", DEVICE_NAME, status);
                return status;
            }
            SleepMSecond(DELAY_PWDN);
        }
    }

    return status;
}


static NTSTATUS GetDsdtSettings(
    IN PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG value = 0;
    ULONG count = 0;
    ULONG i;
    if (!pDevExt)
    {
        return STATUS_INVALID_PARAMETER;
    }

    RtlSecureZeroMemory(&pDevExt->DsdtSettings, sizeof(DSDT_CAMERA_SETTINGS));

    //Get SSDB buffer from BIOS
    NTSTATUS statusACPI = GetAcpiSsdbData(pDevExt->DeviceHandle,
    POOL_TAG,
    sizeof(pDevExt->SensorDriverView),
    &pDevExt->SensorDriverView);
    if (!(NT_SUCCESS(statusACPI)))
    {
        DoTraceMessage(FLAG_ERROR, "%s GetAcpiSsdbData failed with status 0x%x\n", DEVICE_NAME, statusACPI);
    }
    else
    {
        pDevExt->SKU = *(SNSR_SKU*) &(pDevExt->SensorDriverView.SensorCardSKU);
        ConvertEndianForULONG((ULONG*)&pDevExt->SensorDriverView.LinkDelayParameters, sizeof(LINK_DELAY_PARAMETERS) / sizeof(ULONG));

        DoTraceMessage(FLAG_LOG, "%s LinkUsed:0x%x, LanesUsed:0x%x, LanesDiv:0x%x, SensorCardSKU:0x%x, SKU.vendor:0x%x, SKU.crd:0x%x\n", 
                                    DEVICE_NAME, 
                                    pDevExt->SensorDriverView.LinkUsed,
                                    pDevExt->SensorDriverView.LanesUsed,
                                    pDevExt->SensorDriverView.HostControllerGlobal.G_LanesClockDivision,
                                    pDevExt->SensorDriverView.SensorCardSKU,
                                    pDevExt->SKU.vendor,
                                    pDevExt->SKU.crd);

        if(pDevExt->SensorDriverView.Version == SENSOR_INTERFACE_VER1)
        {
            DoTraceMessage(FLAG_LOG, "%s PrivacyLED: 0x%x, CamPosition = 0x%x", DEVICE_NAME, 
            pDevExt->SensorDriverView.SensorDriverSubView.PrivacyLED, pDevExt->SensorDriverView.SensorDriverSubView.CamPosition);
        }

    }
    
    // I2C
    status = GetAcpiDsmData(pDevExt->DeviceHandle,
                            POOL_TAG, (PUCHAR)&DsmI2c, 0, 1, 0, sizeof(ULONG), &count);
    CHECK_STATUS(status);
    pDevExt->DsdtSettings.I2c.Count = count;
    for(i = 0; i < count; i ++)
    {
        status = GetAcpiDsmData(pDevExt->DeviceHandle,
                            POOL_TAG, (PUCHAR)&DsmI2c, 0, 2 + i, 0, sizeof(ULONG), &value);
        CHECK_STATUS(status);
        pDevExt->DsdtSettings.I2c.I2cSet[i].AsUlong = value;
    }

    // GPIO
    status = GetAcpiDsmData(pDevExt->DeviceHandle,
                            POOL_TAG, (PUCHAR)&DsmGpio, 0, 1, 0, sizeof(ULONG), &count);
    CHECK_STATUS(status);
    pDevExt->DsdtSettings.Gpio.Count = count;
    for (i = 0; i < count; i++)
    {
        status = GetAcpiDsmData(pDevExt->DeviceHandle,
                                POOL_TAG, (PUCHAR)&DsmGpio, 0, 2 + i, 0, sizeof(ULONG), &value);
        CHECK_STATUS(status);
        pDevExt->DsdtSettings.Gpio.GpioSet[i].AsUlong = value;
    }

    // Sensor Module
    status = GetAcpiDsmData(pDevExt->DeviceHandle,
                            POOL_TAG,
                            (PUCHAR)&DsmSnsrModule,
                            0,
                            1,
                            0,
                            STRING_LENGTH,
                            &pDevExt->DsdtSettings.ModuleName[0]);
    if (status != STATUS_SUCCESS) 
    { 
        RtlStringCbCopyA((CHAR*)pDevExt->DsdtSettings.ModuleName, sizeof(pDevExt->DsdtSettings.ModuleName), "UNKNOWN");
        goto done;
    }

    //// WA for LKF A1 RVP BIOS---> Change IMX351 sensor module name to 16N02B.
    if (_stricmp((CHAR*)pDevExt->DsdtSettings.ModuleName, "F16N02BPO") == 0)
    {
        RtlStringCbCopyA((CHAR*)pDevExt->DsdtSettings.ModuleName, sizeof(pDevExt->DsdtSettings.ModuleName), "F16N02B");
    }
    else if (_stricmp((CHAR*)pDevExt->DsdtSettings.ModuleName, "F16N02BPO1") == 0)
    {
        RtlStringCbCopyA((CHAR*)pDevExt->DsdtSettings.ModuleName, sizeof(pDevExt->DsdtSettings.ModuleName), "F16N02B1");
    }
    DoTraceMessage(FLAG_LOG, "%s Get DSM Sensor Module %s\n", DEVICE_NAME, (CHAR*)pDevExt->DsdtSettings.ModuleName);

    //platform info
    DoTraceMessage(FLAG_LOG, "%s Get DSM Platform type %d,sub type:%d\n", DEVICE_NAME, pDevExt->SensorDriverView.SensorDriverSubView.Platform,
                                                                                       pDevExt->SensorDriverView.SensorDriverSubView.PlatformSub);

    // VCM type
    pDevExt->SsVcm.VcmType = pDevExt->SensorDriverView.SensorDriverSubView.VcmType;
    //WA for LKF FPGA + KBL RVP as the bios menu do not support the new VCM
    if (pDevExt->PlatformInfo.hwType == HW_FPGA)
    {
        const char *SensorName = DEVICE_NAME;
        if (_stricmp(SensorName, "IMX351") == 0)
            pDevExt->SsVcm.VcmType = 0x10;//VCM_BU64297GWZ
    }
    DoTraceMessage(FLAG_LOG, "%s Get DSM VCM type %d\n", DEVICE_NAME, pDevExt->SsVcm.VcmType);

    //Rom type
    //WA for LKF FPGA + KBL RVP as the bios menu do not support the new EEPROM
    if (pDevExt->PlatformInfo.hwType == HW_FPGA)
    {
        const char *SensorName = DEVICE_NAME;
        if ((_stricmp(SensorName, "IMX351") == 0) && (_stricmp((CHAR*)pDevExt->DsdtSettings.ModuleName, "F16N02B") == 0
                                                    || _stricmp((CHAR*)pDevExt->DsdtSettings.ModuleName, "F16N02BPO") == 0))
            pDevExt->SensorDriverView.SensorDriverSubView.ROMType = 0x11;//CAT24C64
    }
    pDevExt->DsdtSettings.RomType = pDevExt->SensorDriverView.SensorDriverSubView.ROMType;
    DoTraceMessage(FLAG_LOG, "%s Get ROM type %d\n", DEVICE_NAME, pDevExt->DsdtSettings.RomType);

    // sensor calibration data
    if (pDevExt->DsdtSettings.RomType == ROM_OTP_ACPI_ACPI || pDevExt->DsdtSettings.RomType == ROM_ACPI)
    {
        int j = 0;
        PNVM_CONTEXT pSsNvm = &pDevExt->SsNvm;
        DoTraceMessage(FLAG_LOG, "%s Sensoreee RomType is %d\n", DEVICE_NAME, pDevExt->DsdtSettings.RomType);
        status = GetAcpiDsmData(pDevExt->DeviceHandle,
                                POOL_TAG,
                                (PUCHAR)&DsmSnsrCalibration,
                                0,
                                1,
                                0,
                                CALIBRATION_LENGTH,
                                &pSsNvm->NvmBuffer[0]);
        if (status != STATUS_SUCCESS) 
        { 
            DoTraceMessage(FLAG_ERROR, "%s Get DSM calibration data failed with status:0x%x\n", DEVICE_NAME, status);
            RtlStringCbCopyA((CHAR*)pSsNvm->NvmBuffer, 8, "UNKNOWN");
            if (status == STATUS_BUFFER_OVERFLOW)
                DoTraceMessage(FLAG_ERROR, "%s Sensor Calibration Data in BIOS can not be more than %d\n", DEVICE_NAME, CALIBRATION_LENGTH - 1);
            goto done;
        }
        
        DoTraceMessage(FLAG_LOG, "%s Get DSM Sensor Calibration Data with status:0x%x,cal data:", DEVICE_NAME, status);
    
        for (j = 0; j < EEPROM_VALID_SIZE / 16; j ++)
        {
            DoTraceMessage(FLAG_LOG, "0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x,0x%02x",
                                      pSsNvm->NvmBuffer[16*j + 0],  pSsNvm->NvmBuffer[16*j + 1],  pSsNvm->NvmBuffer[16*j + 2], pSsNvm->NvmBuffer[16*j + 3],
                                      pSsNvm->NvmBuffer[16 * j + 4], pSsNvm->NvmBuffer[16 * j + 5], pSsNvm->NvmBuffer[16 * j + 6], pSsNvm->NvmBuffer[16 * j + 7],
                                      pSsNvm->NvmBuffer[16 * j + 8], pSsNvm->NvmBuffer[16 * j + 9], pSsNvm->NvmBuffer[16 * j + 10], pSsNvm->NvmBuffer[16 * j + 11],
                                      pSsNvm->NvmBuffer[16 * j + 12], pSsNvm->NvmBuffer[16 * j + 13], pSsNvm->NvmBuffer[16 * j + 14], pSsNvm->NvmBuffer[16 * j + 15]
                                      );
        }
    }
    else
    {
        DoTraceMessage(FLAG_LOG, "%s Sensor RomType is %d,Skip Get DSM Sensor Calibration Data\n", DEVICE_NAME, pDevExt->DsdtSettings.RomType);
    }

    //SKYCAM default is Zero
    //BXT-P Setting: Sensor Mclk Port setting
    //Port A: OSC_CLKOUT_0
    //Port B: OSC_CLKOUT_1
    //Port C: OSC_CLKOUT_2
    pDevExt->MclkPort = pDevExt->SensorDriverView.SensorDriverSubView.MclkPort;

done:
    return status;
}

static NTSTATUS ParseDsdtSettings(
    IN PDEVICE_CONTEXT SensorCtx
    )
{
    UCHAR flashSupport;
    ULONG privacyLed = 0;

    if (!SensorCtx)
    {
        return STATUS_INVALID_PARAMETER;
    }
    memcpy_s(&SensorCtx->SensorCaps, sizeof(SNSR_CAPS), &DefaultSensorCaps, sizeof(SNSR_CAPS));
    memcpy_s(&SensorCtx->SensorMipiConfig, sizeof(SNSR_MIPI_CONFIG), &DefaultSensorMipiConfig, sizeof(SNSR_MIPI_CONFIG));

    // flash support
    flashSupport = SensorCtx->SensorDriverView.SensorDriverSubView.FlashSupport;

    if (FLASH_DISABLE == flashSupport)
        SensorCtx->SensorCaps.FlashSupport = FALSE;
    else if (FLASH_ENABLE == flashSupport)
        SensorCtx->SensorCaps.FlashSupport = TRUE;

    DoTraceMessage(FLAG_LOG, "%s DSM flash flag:%d", DEVICE_NAME, flashSupport);

    // mipi port, lane number
    if (SensorCtx->SensorDriverView.SensorDriverSubView.MipiDefine == MIPI_IN_ACPI)
    {
        SensorCtx->SensorMipiConfig.MipiPort = SensorCtx->SensorDriverView.LinkUsed;
        SensorCtx->SensorMipiConfig.MipiLane = SensorCtx->SensorDriverView.LanesUsed;
    }
    SensorCtx->SensorMipiConfig.OutputFormat = DefaultSensorMipiConfig.OutputFormat;

    // nvram 
    ParseNvram(&SensorCtx->SsNvm, SensorCtx->SensorDriverView.SensorDriverSubView.ROMType, 0, 0, 0);

    // orientation, RS4 and after using orientation bits PLDB block below not useful now
    if (SensorCtx->SensorDriverView.SensorDriverSubView.Degree == 180)
        SensorCtx->Orientation = DEGREE_180;
    else
        SensorCtx->Orientation = SensorCtx->SensorDriverView.SensorDriverSubView.Degree;
    
    DoTraceMessage(FLAG_LOG, "%s SensorDriverView.SensorDriverSubView.Degree:%d, SensorCtx->Orientation:%d", DEVICE_NAME, 
                                 SensorCtx->SensorDriverView.SensorDriverSubView.Degree, SensorCtx->Orientation);

    SensorCtx->SensorConf.ledConf.sensor_interface_version = SensorCtx->SensorDriverView.Version;
    privacyLed = SensorCtx->SensorDriverView.SensorDriverSubView.PrivacyLED;

    if(SensorCtx->SensorDriverView.Version == SENSOR_INTERFACE_VER1)
    {
        SensorCtx->SensorConf.ledConf.CamPosition = SensorCtx->SensorDriverView.SensorDriverSubView.CamPosition;
        SensorCtx->SensorConf.ledConf.privacyLed = PRIVACY_LED_B;
        switch(privacyLed)
        {
        case PRIVACY_LED_DEFAULT:
            if (SensorCtx->SensorConf.ledConf.CamPosition == CAMERA_POSITION_1 ||
            SensorCtx->SensorConf.ledConf.CamPosition == CAMERA_POSITION_3)
            {
                 SensorCtx->SensorConf.ledConf.privacyLed = PRIVACY_LED_A;
                 SensorCtx->SensorConf.ledConf.privacyLedCurrent = LED_CURRENT_16mA;
            }
            else if(SensorCtx->SensorConf.ledConf.CamPosition == CAMERA_POSITION_2)
            {
                SensorCtx->SensorConf.ledConf.privacyLed = PRIVACY_LED_B;
                SensorCtx->SensorConf.ledConf.privacyLedCurrent = LED_CURRENT_16mA;
            }
            else
            {
                DoTraceMessage(FLAG_LOG, "Error: wrong BIOS Link configuration for CamPosition");
            }
            break;
        case PRIVACY_LED_A_16mA:
            SensorCtx->SensorConf.ledConf.privacyLed = PRIVACY_LED_A;
            SensorCtx->SensorConf.ledConf.privacyLedCurrent = LED_CURRENT_16mA;
            break;

        case PRIVACY_LED_B_2mA:
            SensorCtx->SensorConf.ledConf.privacyLedCurrent = LED_CURRENT_2mA;
            break;

        case PRIVACY_LED_B_4mA:
            SensorCtx->SensorConf.ledConf.privacyLedCurrent = LED_CURRENT_4mA;
            break;

        case PRIVACY_LED_B_8mA:
            SensorCtx->SensorConf.ledConf.privacyLedCurrent = LED_CURRENT_8mA;
            break;

        case PRIVACY_LED_B_16mA:
            SensorCtx->SensorConf.ledConf.privacyLedCurrent = LED_CURRENT_16mA;
            break;

        default:
            break;

        }

        SensorCtx->SensorConf.Votage_rail = SensorCtx->SensorDriverView.SensorDriverSubView.Voltagerail;
            
        DoTraceMessage(FLAG_LOG, "%s SensorConf.privacyLed:%d,SensorConf.privacyLedCurrent:%d, Votage_rail: %d", DEVICE_NAME, 
                        SensorCtx->SensorConf.ledConf.privacyLed,SensorCtx->SensorConf.ledConf.privacyLedCurrent, SensorCtx->SensorConf.Votage_rail);
 
    }

    return STATUS_SUCCESS;
}


static void InitInterfaceVcmNvm(
    PDEVICE_CONTEXT pDevExt
    )
{
    pDevExt->SensorInterface.SetFocusPos     = Cmd_SetPos;
    pDevExt->SensorInterface.GetFocusPos     = Cmd_GetPos;
    pDevExt->SensorInterface.GetFocusStatus  = Cmd_GetStatus;
    pDevExt->SensorInterface.GetFocusHPStatus = Cmd_GetHPStatus;
    pDevExt->SensorInterface.SetVcmConfiguration = Cmd_SetConfig;
    pDevExt->SensorInterface.SetEEPRom = Cmd_NvmWrite;
    pDevExt->SensorInterface.GetEEPRom = Cmd_NvmRead;
    pDevExt->SensorInterface.GetCalibrationData = Cmd_GetNvmData;
}

void InitCameraDriverRegistryFlags(
    IN PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG ulValue = 0;
    ULONG ulBufLen = sizeof(ulValue);
    UNICODE_STRING CameraDriverRegistryPath;
    BOOLEAN bPortSip2Sip3Enabled = FALSE;

    if (!pDevExt)
    {
        return;
    }

    RtlInitUnicodeString(&CameraDriverRegistryPath, CAMERA_DRIVER_REGISTRY_PATH);
    status = OS_ReadRegistry(&CameraDriverRegistryPath, (PUCHAR)"EnablePortSip2Sip3", REG_DWORD, (PUCHAR)&ulValue, &ulBufLen, POOL_TAG);
    if (NT_SUCCESS(status))
        bPortSip2Sip3Enabled = (BOOLEAN)ulValue;
    if (bPortSip2Sip3Enabled)
    {
        // For 4 camera case on LKF1 FPGA, lower the sensor fps.
        pDevExt->IsLowerFpsNeeded = bPortSip2Sip3Enabled;
        DoTraceMessage(FLAG_LOG, "%s Use lower fps sensor setting.\n", DEVICE_NAME);
    }
}

void SsInitRegistry(
    IN PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG ulValue = 0;
    ULONG ulBufLen = sizeof(ulValue);

    if (!pDevExt)
    {
        return;
    }

    pDevExt->Key_IsRegCheck = FALSE;
    pDevExt->Key_MipiLanes = 0;
#ifndef SKY_CAM
    pDevExt->Key_EnableEmbeddedData = REGISTRY_INVALID;
#endif

#ifdef CAMERA_SENSOR_CONFIGURABLE
    status = OS_ReadRegistry(&g_RegistryPath, (PUCHAR)"RegCheck", REG_DWORD, (PUCHAR)&ulValue, &ulBufLen, POOL_TAG);
    if (NT_SUCCESS(status))
        pDevExt->Key_IsRegCheck = ulValue ? TRUE : FALSE;
    
    status = OS_ReadRegistry(&g_RegistryPath, (PUCHAR)"MipiLanes", REG_DWORD, (PUCHAR)&ulValue, &ulBufLen, POOL_TAG);
    if (NT_SUCCESS(status))
        pDevExt->Key_MipiLanes = (UINT8)ulValue;

    status = OS_ReadRegistry(&g_RegistryPath, (PUCHAR)"MipiPort", REG_DWORD, (PUCHAR)&ulValue, &ulBufLen, POOL_TAG);
   if (NT_SUCCESS(status))
        pDevExt->Key_MipiPort = (UINT8)ulValue;

    status = OS_ReadRegistry(&g_RegistryPath, (PUCHAR)"MipiDataFormat", REG_DWORD, (PUCHAR)&ulValue, &ulBufLen, POOL_TAG);
    if (NT_SUCCESS(status))
        pDevExt->Key_MipiDataFormat = (UINT8)ulValue;

    status = OS_ReadRegistry(&g_RegistryPath, (PUCHAR)"MipiMBps", REG_DWORD, (PUCHAR)&ulValue, &ulBufLen, POOL_TAG);
    if (NT_SUCCESS(status))
        pDevExt->Key_MipiMbps = (INT32)ulValue;

#endif

    ulValue = 0xff;
    status = OS_ReadRegistry(&g_RegistryPath, (PUCHAR)"EnableEmbeddedData", REG_DWORD, (PUCHAR)&ulValue, &ulBufLen, POOL_TAG);
    if (NT_SUCCESS(status) && (ulValue != 0xff))
#ifdef SKY_CAM
        pDevExt->IsEmbeddedDataEnabled = ulValue ? TRUE : FALSE;
#else
        pDevExt->Key_EnableEmbeddedData = ulValue ? TRUE : FALSE;
#endif

    pDevExt->DumpNVMData = FALSE;
    status = OS_ReadRegistry(&g_RegistryPath, (PUCHAR)"DumpNVMData", REG_DWORD, (PUCHAR)&ulValue, &ulBufLen, POOL_TAG);
    if (NT_SUCCESS(status))
         pDevExt->DumpNVMData = ulValue ? TRUE : FALSE;

    pDevExt->IsTestModeEnabled = SENSOR_TESTMODE_NOP;
    status = OS_ReadRegistry(&g_RegistryPath, (PUCHAR)"EnableSnrTestMode", REG_DWORD, (PUCHAR)&ulValue, &ulBufLen, POOL_TAG);
    if (NT_SUCCESS(status))
        pDevExt->IsTestModeEnabled = ulValue ? SENSOR_TESTMODE_ENABLE : SENSOR_TESTMODE_DISABLE;
    InitCameraDriverRegistryFlags(pDevExt);
}

void SsInitContextCommon(PDEVICE_CONTEXT deviceCtx)
{
    if (!deviceCtx)
    {
        return;
    }

    DoTraceMessage(FLAG_LOG, "%s SsInitContextCommon\n", DEVICE_NAME);

    deviceCtx->Guid = SENSOR_GUID;
    deviceCtx->IsEmbeddedDataEnabled = FALSE;
    deviceCtx->IsPdafDataEnabled = FALSE;
    deviceCtx->DriverStage = DRIVER_STAGE_LOADING;
    deviceCtx->PowerOnStatus = PWR_DOWN;
    deviceCtx->ResIndex = 0;
    deviceCtx->Mode = PreviewMode;
    deviceCtx->Binning = BINNING_MODE_NONE;
    deviceCtx->MipiBps = 0;
    deviceCtx->SensorMipiConfig.MipiLane = DefaultSensorMipiConfig.MipiLane;

#ifdef SKY_CAM
    deviceCtx->ControlLogicInterface = NULL;
    deviceCtx->SensorNotificationEntry = NULL;
    deviceCtx->ControlLogicDeviceObject = NULL;
    deviceCtx->SsVcm.VCMHomePostion = 250;//set default home postion, different AF camera module have different value
    RtlSecureZeroMemory(&deviceCtx->SensorConf, sizeof(SENSOR_CONF));
#else
    deviceCtx->SensorInterface.GetPDAFCaps = NULL;
    deviceCtx->SensorInterface.SetSensorCalibrationData = NULL;
#endif
    deviceCtx->SensorInterface.GetPDAFResolutions = NULL;
    deviceCtx->SensorInterface.SetMultipleExposure = NULL;

    SsGpioZeroInit(&deviceCtx->GpioDev, deviceCtx->DeviceHandle, DEVICE_NAME);
    SsI2cZeroInit(&deviceCtx->I2cDev, deviceCtx->DeviceHandle, DEVICE_NAME);
    SsVcmZeroInit(&deviceCtx->SsVcm, deviceCtx->DeviceHandle, DEVICE_NAME, 
        &deviceCtx->SettingEvent,
        &deviceCtx->I2cDev);
    SsNvmZeroInit(&deviceCtx->SsNvm, deviceCtx->DeviceHandle, DEVICE_NAME, 
        &deviceCtx->SettingEvent,
        &deviceCtx->I2cDev,
        (ULONG*)deviceCtx
        );

    if (!NT_SUCCESS(GetDsdtSettings(deviceCtx)))
    {
        DoTraceMessage(FLAG_ERROR, "%s Get DSDT settings failed!\n", DEVICE_NAME);
        return;
    }
    ParseDsdtSettings(deviceCtx);

    // Get PLD
    if (!NT_SUCCESS(Cmd_GetPld(deviceCtx, &deviceCtx->Pld)))
    {
        DoTraceMessage(FLAG_ERROR, "%s Get PLD failed!\n", DEVICE_NAME);
        return;
    }

    SsInitExportResTable(deviceCtx);

    InitContextVcm(&deviceCtx->SsVcm);
    InitContextNvram(&deviceCtx->SsNvm);
    InitInterfaceVcmNvm(deviceCtx);
}

void SsInitContextCommonPost(PDEVICE_CONTEXT deviceCtx)
{
    UNREFERENCED_PARAMETER(deviceCtx);
#ifdef AE_EXACT_CONFIG
    expInit(deviceCtx);
#endif
}

static NTSTATUS SensorHwRecovery(
    PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(pDevExt)
            
    // power off current camera module
    status = SensorPower(pDevExt, FALSE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SensorPower Off Fail!\n", DEVICE_NAME);
        goto done;
    }
    SleepMSecond(20);
    // power on current camera module
    status = SensorPower(pDevExt, TRUE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SensorPower On Fail!\n", DEVICE_NAME);
        goto done;
    }
done:
    return status;
}

#define MAX_RELOAD_COUNT            5

#ifdef SKY_CAM

static NTSTATUS Startup(
    PDEVICE_CONTEXT SensorCtx
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    if (!SensorCtx)
    {
        return STATUS_INVALID_PARAMETER;
    }
    
    UINT8 reload;

    DoTraceMessage(FLAG_LOG, "%s Startup (enters) \n", DEVICE_NAME);
    
    // power on current camera module
    status = SensorPower(SensorCtx, TRUE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SensorPower On Fail!\n", DEVICE_NAME);
        goto done;
    }

    // update power status
    if (SensorCtx->PowerOnStatus == PWR_DOWN)
        SensorCtx->PowerOnStatus = PWR_UP;

    ASSERT(NT_SUCCESS(status));

    if((SensorCtx->DriverStage == DRIVER_STAGE_LOADING) 
        || _stricmp((CHAR*)SensorCtx->DsdtSettings.ModuleName, "CM500RF05SW700") == 0
        || _stricmp((CHAR*)SensorCtx->DsdtSettings.ModuleName, "NSMM4D5") == 0
        || _stricmp((CHAR*)SensorCtx->DsdtSettings.ModuleName, "START2FRONT") == 0)
    {
        // check camera module: cmos identify and vcm i2c
        reload = MAX_RELOAD_COUNT;
        // module_check:
        SleepMSecond(20);
        status = ModuleCheck(SensorCtx);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s Module Check Fail\n", DEVICE_NAME);
            goto done;
        }
        SensorCtx->DriverStage = DRIVER_STAGE_WORKING;
        DoTraceMessage(FLAG_LOG, "%s Module Check Successfully\n", DEVICE_NAME);
    }

    reload = MAX_RELOAD_COUNT;
    while (reload > 0)
    {
        reload--;
        // Reset sensor if need. todo
        if (!NT_SUCCESS(status))
        {
            DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "%s Reset sensor\n", DEVICE_NAME);
            DoTraceMessage(FLAG_ERROR, "%s Failed to init, reload %d\n", DEVICE_NAME, reload);
        }
        // init cmos sensor
        status = InitCmos(SensorCtx);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s InitCmos fail\n", DEVICE_NAME);
            continue;
        }
        // init focus vcm
        status = InitVcm(&SensorCtx->SsVcm);
        if (!NT_SUCCESS(status))
        {
            DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "%s InitVcm fail\n", DEVICE_NAME);
            DoTraceMessage(FLAG_ERROR, "%s InitVcm fail\n", DEVICE_NAME);
            continue;
        }
        // init otp/eeprom..
        status = InitRom(&SensorCtx->SsNvm);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s InitRom fail\n", DEVICE_NAME);
            continue;
        }
        // All initialize success, break.
        break;
    }
    if (!NT_SUCCESS(status) && reload == 0)
    {
        DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "%s Startup fail\n", DEVICE_NAME);
    }
done:
    ReleaseI2c(SensorCtx);
    DoTraceMessage(FLAG_LOG, "%s Startup (exit) 0x%x\n", DEVICE_NAME, status);
    return status;
}

#else
#if defined(OTM_SENSOR_CONTROL_LOGIC)

static NTSTATUS Startup(
    PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    UINT8 reload;

    if (!pDevExt)
    {
        return STATUS_INVALID_PARAMETER;
    }

    DoTraceMessage(FLAG_LOG, "%s Startup (enters) \n", DEVICE_NAME);
    
    // power on current camera module
    status = SensorPower(pDevExt, TRUE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SensorPower On Fail!\n", DEVICE_NAME);
        goto done;
    }

    if (pDevExt->DriverStage == DRIVER_STAGE_LOADING)
    {
        if (!(pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_BXT && pDevExt->SKU.crd == SKU_CRG_VALUE))
        {
            // check camera module: cmos identify
            reload = MAX_RELOAD_COUNT;
            //module_check:
            SleepMSecond(20);
            status = ModuleCheck(pDevExt);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s Module Check Fail\n", DEVICE_NAME);
                goto done;
            }
        }

        pDevExt->DriverStage = DRIVER_STAGE_WORKING;
        DoTraceMessage(FLAG_LOG, "%s Module Check Successfully\n", DEVICE_NAME);
    }

    // update power status
    if (pDevExt->PowerOnStatus == PWR_DOWN)
        pDevExt->PowerOnStatus = PWR_UP;

    ASSERT(NT_SUCCESS(status));

    reload = MAX_RELOAD_COUNT;
    while (reload > 0)
    {
        reload--;
        // Reset sensor if need. todo
        if (!NT_SUCCESS(status))
        {
            DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "%s Reset sensor\n", DEVICE_NAME);
            DoTraceMessage(FLAG_ERROR, "%s Failed to init, reload %d\n", DEVICE_NAME, reload);
        }
        // init cmos sensor
        status = InitCmos(pDevExt);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s InitCmos fail\n", DEVICE_NAME);
            continue;
        }
        // init focus vcm
        status = InitVcm(&pDevExt->SsVcm);
        if (!NT_SUCCESS(status))
        {
            DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "%s InitVcm fail\n", DEVICE_NAME);
            DoTraceMessage(FLAG_ERROR, "%s InitVcm fail\n", DEVICE_NAME);
            continue;
        }
        // init otp/eeprom..
        status = InitRom(&pDevExt->SsNvm);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s InitRom fail\n", DEVICE_NAME);
            continue;
        }
        else
        {

            if (pDevExt->SensorDriverView.SensorDriverSubView.Platform != PLATFORM_SKC)
            {
                if (pDevExt->DumpNVMData)
                {
                    if (pDevExt->SsNvm.NvmType == NVM_OTP)
                        DumpNVM(&pDevExt->SsNvm, OTP_VALID_SIZE);
                    else
                        DumpNVM(&pDevExt->SsNvm, EEPROM_VALID_SIZE);
                }
            }
        }
        // All initialize success, break.
        break;
    }
    if (!NT_SUCCESS(status) && reload == 0)
    {
        DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "%s Startup fail\n", DEVICE_NAME);
    }
done:
    ReleaseI2c(pDevExt);
    DoTraceMessage(FLAG_LOG, "%s Startup (exit) 0x%x\n", DEVICE_NAME, status);
    return status;
}

#else
NTSTATUS SensorPower(
    PDEVICE_CONTEXT pDevExt,
    BOOLEAN PowerOn
    )
{
    NTSTATUS status = STATUS_SUCCESS;

#if defined(CHT_SLE)
    UNREFERENCED_PARAMETER(pDevExt);
    UNREFERENCED_PARAMETER(PowerOn);
#else

    if (PowerOn)
    {
        // enable camera module
        status = SsPowerGpio(pDevExt, TRUE);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s SsPowerGpio On Fail!\n", DEVICE_NAME);
            goto done;
        }
    }
    else
    {
        // gpio off
        status = SsPowerGpio(pDevExt, FALSE);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s Failed to power off sensor\n", DEVICE_NAME);
            goto done;
        }
    }
done:
#endif
    DoTraceMessage(FLAG_ERROR, "%s SensorPower (return 0x%x)\n", DEVICE_NAME, status);
    return status;
}

NTSTATUS SensorPowerAllOff(
    PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(pDevExt)      

    // close all vprog and flisclock
    status = SsPowerGpio(pDevExt, FALSE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SsPowerGpio Off Fail!\n", DEVICE_NAME);
        goto done;
    }
    SleepMSecond(5);
done:
    return status;
}

static NTSTATUS Startup(
    PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;
#ifndef BXT
    NTSTATUS sts = STATUS_SUCCESS;
#endif
    if (!pDevExt)
    {
        return STATUS_INVALID_PARAMETER;
    }
    
#if defined(CHT_SLE)
    UNREFERENCED_PARAMETER(sts);
    if (pDevExt->PowerOnStatus == PWR_DOWN)
        pDevExt->PowerOnStatus = PWR_UP;
#else
    DoTraceMessage(FLAG_LOG, "%s Startup (enters)\n", DEVICE_NAME);
    
    // power on current camera module
    status = SensorPower(pDevExt, TRUE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SensorPower On Fail!\n", DEVICE_NAME);
        goto done;
    }

#ifdef BXT
    // update power status
    if (pDevExt->PowerOnStatus == PWR_DOWN)
        pDevExt->PowerOnStatus = PWR_UP;
#else
    UINT8 reload;

    if (pDevExt->DriverStage == DRIVER_STAGE_LOADING)
    {
        // check camera module: cmos identify and vcm i2c
        reload = MAX_RELOAD_COUNT;
module_check:
        SleepMSecond(20);
        status = ModuleCheck(pDevExt);
        if (!NT_SUCCESS(status))
        {
            if (reload)
            {
                status = SensorHwRecovery(pDevExt);
                if (!NT_SUCCESS(status))
                    goto done;
                reload --;
                DoTraceMessage(FLAG_ERROR, "%s Module Check Fail, but hw recovery %dst\n", 
                    DEVICE_NAME, MAX_RELOAD_COUNT - reload);
                goto module_check;
            }
            DoTraceMessage(FLAG_ERROR, "%s Module Check Fail\n", DEVICE_NAME);
            goto done;
        }
        pDevExt->DriverStage = DRIVER_STAGE_WORKING;
        DoTraceMessage(FLAG_LOG, "%s Module Check Successfully\n", DEVICE_NAME);
    }

    // update power status
    if (pDevExt->PowerOnStatus == PWR_DOWN)
        pDevExt->PowerOnStatus = PWR_UP;

    ASSERT(NT_SUCCESS(status));

    reload = MAX_RELOAD_COUNT;
    while (reload > 0)
    {
        reload--;
        // Reset sensor if need.
        if (!NT_SUCCESS(status))
        {
            int i = 0;
            DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "%s Reset sensor\n", DEVICE_NAME);
            DoTraceMessage(FLAG_ERROR, "%s Failed to init, reload %d\n", DEVICE_NAME, reload);
            status = SsPowerGpio(pDevExt, FALSE);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s SsPowerGpio off fail\n", DEVICE_NAME);
                goto done;
            }
            // Here we set Gpio twice, which is a temporarily patch.
            // On 9390, imx175 some times does not work if we only set GPIO once.
            for (i = 0; i < 2; i++)
            {
                SleepMSecond(5);
                status = SsPowerGpio(pDevExt, TRUE);
                if (!NT_SUCCESS(status))
                {
                    DoTraceMessage(FLAG_ERROR, "%s SsPowerGpio on fail\n", DEVICE_NAME);
                    goto done;
                }
            }
        }
        // init cmos sensor
        status = InitCmos(pDevExt);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s InitCmos fail\n", DEVICE_NAME);
            continue;
        }
        // init focus vcm
        sts = InitVcm(&pDevExt->SsVcm);
        if (!NT_SUCCESS(sts))
        {
            DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "%s InitVcm fail\n", DEVICE_NAME);
            DoTraceMessage(FLAG_ERROR, "%s InitVcm fail\n", DEVICE_NAME);
            continue;
        }
        // init otp/eeprom..
        sts = InitRom(&pDevExt->SsNvm);
        if (!NT_SUCCESS(sts))
        {
            DoTraceMessage(FLAG_ERROR, "%s InitRom fail\n", DEVICE_NAME);
            continue;
        }
        // All initialize success, break.
        break;
    }
    if (!NT_SUCCESS(status) && reload == 0)
    {
        DbgPrintEx(DPFLTR_IHVDRIVER_ID, DPFLTR_ERROR_LEVEL, "%s Startup fail\n", DEVICE_NAME);
    }
#endif
done:
    ReleaseI2c(pDevExt);
#endif
    DoTraceMessage(FLAG_LOG, "%s Startup (exit) 0x%x\n", DEVICE_NAME, status);
    return status;
}
#endif

#endif

static NTSTATUS Shutdown(
    PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    
    CHECK_POINTER(pDevExt)

#ifdef PIXTER_DEFINE
    UnInitCmos(pDevExt);
#endif
    // power off current camera module
    status = SensorPower(pDevExt, FALSE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SensorPower Off Fail!\n", DEVICE_NAME);
    }

    ReleaseI2c(pDevExt);
    pDevExt->PowerOnStatus = PWR_DOWN;
done:
    return status;
}

NTSTATUS Cmd_Idle(
    PDEVICE_CONTEXT pDevExt
    )
{
    DoTraceMessage(FLAG_LOG, "%s Cmd_Idle+\n", DEVICE_NAME);

    WdfDeviceResumeIdle(pDevExt->DeviceHandle);
#if NTDDI_VERSION > NTDDI_WINBLUE

    if (pDevExt->ControlLogicInterface)
    {
        pDevExt->ControlLogicInterface->SetIdle(
            pDevExt->ControlLogicInterface->InterfaceHeader.Context);

        pDevExt->ControlLogicInterface->InterfaceHeader.InterfaceDereference(
            pDevExt->ControlLogicInterface->InterfaceHeader.Context);
    }
#endif
    DoTraceMessage(FLAG_LOG, "%s Cmd_Idle-\n", DEVICE_NAME);
    return STATUS_SUCCESS;
}


NTSTATUS Cmd_Active(
    PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS  status = STATUS_SUCCESS;

    DoTraceMessage(FLAG_LOG, "%s Cmd_Active+\n", DEVICE_NAME);
#if NTDDI_VERSION > NTDDI_WINBLUE
    if (pDevExt->ControlLogicInterface)
    {

        pDevExt->ControlLogicInterface->InterfaceHeader.InterfaceReference(
            pDevExt->ControlLogicInterface->InterfaceHeader.Context);

        pDevExt->ControlLogicInterface->SetActive(
            pDevExt->ControlLogicInterface->InterfaceHeader.Context);
    }
#endif
    status = WdfDeviceStopIdle(pDevExt->DeviceHandle, TRUE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s WdfDeviceStopIdle Failed, Current Reference Count %d\n", DEVICE_NAME,
            pDevExt->ReferenceCount);
    }

    DoTraceMessage(FLAG_LOG, "%s Cmd_Active-\n", DEVICE_NAME);
    return status;
}

NTSTATUS Cmd_Startup(PDEVICE_CONTEXT pDevExt)
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(pDevExt)      

    DoTraceMessage(FLAG_LOG, "%s Cmd_Startup+ x\n", DEVICE_NAME);

#ifdef SKY_CAM
#if NTDDI_VERSION <= NTDDI_WINBLUE
    if (pDevExt->ControlLogicInterface)
    {
        pDevExt->ControlLogicInterface->InterfaceHeader.InterfaceReference(
            pDevExt->ControlLogicInterface->InterfaceHeader.Context);

        pDevExt->ControlLogicInterface->SetActive(
            pDevExt->ControlLogicInterface->InterfaceHeader.Context);
    }
#endif
#endif

    status = Startup(pDevExt);
    if (!NT_SUCCESS(status))
        goto done;
done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_Startup- returns with code: 0x%x\n", DEVICE_NAME, status);
    return status;
}

NTSTATUS Cmd_Shutdown(PDEVICE_CONTEXT pDevExt)
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(pDevExt)

    DoTraceMessage(FLAG_LOG, "%s Cmd_Shutdown+ x\n", DEVICE_NAME);

    // Reset focus vcm
    ResetVcmPosition(&pDevExt->SsVcm);
#ifndef SKY_CAM
    ReleaseRom(&pDevExt->SsNvm);
#endif
    status = Shutdown(pDevExt);
#ifdef SKY_CAM
#if NTDDI_VERSION <= NTDDI_WINBLUE 
    if(pDevExt->ControlLogicInterface)
    {
        pDevExt->ControlLogicInterface->SetIdle(
            pDevExt->ControlLogicInterface->InterfaceHeader.Context);

        pDevExt->ControlLogicInterface->InterfaceHeader.InterfaceDereference(
            pDevExt->ControlLogicInterface->InterfaceHeader.Context);
    }
#endif
#endif
    if (!NT_SUCCESS(status))
        goto done;
done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_Shutdown- returns with code: 0x%x\n", DEVICE_NAME, status);
    return status;
}

NTSTATUS Cmd_GetModeData(
    PDEVICE_CONTEXT pDevExt,
    PULONG Size,
    PUINT8 Buffer    
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER_INVALID(pDevExt)
    CHECK_POINTER_INVALID(Size)
    CHECK_POINTER_INVALID(Buffer)

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetModeData +\n", DEVICE_NAME);

    if (*Size > sizeof(SENSOR_MODE_DATA))
    {
        *Size = sizeof(SENSOR_MODE_DATA);
    }
#ifdef SKY_CAM
    status = KeWaitForSingleObject(&pDevExt->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s sensor_get_intg_factor KeWaitForSingleObject fail, (status 0x%x)\n",
            DEVICE_NAME, status);
        goto invalid;
    }   
#endif

    status = GetTimingFactor(pDevExt, (PSENSOR_MODE_DATA)Buffer);
#ifdef SKY_CAM
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
#endif

invalid:
    DoTraceMessage( FLAG_LOG, "%s Cmd_GetModeData - (size=%d, buffer=0x%p, returns 0x%x)\n", 
        DEVICE_NAME, Size ? *Size : 0, Buffer, status);
    return status;
}

NTSTATUS Cmd_SetResolution(
    PDEVICE_CONTEXT pDevExt,
    SNSR_MODE Mode,
    ULONG Width,
    ULONG Height,
    LONG frameRate
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    int idx;

    DoTraceMessage(FLAG_LOG, "%s Cmd_SetResolution+ Fmt: w=%d, h=%d, fps=%d \n", DEVICE_NAME, Width, Height, frameRate);
    CHECK_POINTER_INVALID(pDevExt)

    if (pDevExt->PowerOnStatus != PWR_UP)
        status = Startup(pDevExt);

#ifdef SKY_CAM
    // Try/Set Fmt
    idx = FindResIndex(Mode, Width, Height, frameRate);
    if(idx == -1)
    {
        status = STATUS_UNSUCCESSFUL;
        goto done;
    }
#if 0
    status = InitResolution(pDevExt, Mode, idx);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s SetResolution Fail! \n", DEVICE_NAME);
        goto done;
    }
#else
    {
        SNSR_RESOLUTION_DEBUG resolution;
        resolution.Width = (UINT16)(Width); resolution.Height = (UINT16)(Height); resolution.CropX = 1000; resolution.CropY = 1000;
        resolution.FPS = (UINT16)(frameRate); resolution.BayerOrder = BAYER_ORDER_GRBG_H_FLIP; resolution.BinningMode = BinningModeAuto;
		resolution.BPP = 10; resolution.NumberOfLanes = pDevExt->SensorDriverView.LanesUsed; resolution.ClockMode = ClockModeSlow;
        //{ (UINT16)(Width), (UINT16)(Height), 1000, 1000, (UINT16)(frameRate), BAYER_ORDER_GRBG_H_FLIP,
        //    BinningModeAuto, 10, SensorCtx->SensorDriverView.LanesUsed, ClockModeSlow };
        status = STATUS_SUCCESS;// IMX135Cmd_SetResolutionDebug(SensorCtx, Mode, resolution);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_LOG, "%s SetResolution Fail! \n", DEVICE_NAME);
            goto done;
        }
    }
#endif
    pDevExt->ResIndex = idx;
    pDevExt->Mode = Mode;
    pDevExt->BankId = 0;
done:
    DoTraceMessage(FLAG_LOG, "%s SetResolution index = %d \n", DEVICE_NAME, pDevExt->ResIndex);
#else
    status = KeWaitForSingleObject(&pDevExt->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%s SetResolution KeWaitForSingleObject fail, (status 0x%x)\n",
            DEVICE_NAME, status);
        goto invalid;
    }

    // Try/Set Fmt
    idx = FindResIndex(Mode, Width, Height, frameRate);
    if(idx == -1)
    {
        status = STATUS_UNSUCCESSFUL;
        goto done;
    }

    status = InitResolution(pDevExt, Mode, idx);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s SetResolution Fail! \n", DEVICE_NAME);
        goto done;
    }

    //Eneable Test Mode
    if (pDevExt->IsTestModeEnabled != SENSOR_TESTMODE_NOP && pDevExt->SensorInterface.SetExtFeature != NULL)
    {
        pDevExt->SensorInterface.SetExtFeature(pDevExt, SNSR_EXT_FEATURE_TESTMODE, pDevExt->IsTestModeEnabled);
    }

    pDevExt->ResIndex = idx;
    pDevExt->Mode = Mode;
    pDevExt->BankId = 0;
    pDevExt->Binning = (UINT8)Sensor_Res[pDevExt->Mode][pDevExt->ResIndex].Binning;
    pDevExt->MipiBps = (INT32)Sensor_Res[pDevExt->Mode][pDevExt->ResIndex].MipiBps;
done:
    DoTraceMessage(FLAG_LOG, "%s SetResolution index = %d \n", DEVICE_NAME, pDevExt->ResIndex);
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
#endif
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_SetResolution- (%d x %d, ) returns with code: 0x%x\n", DEVICE_NAME,
        Width, Height, status);

    return status;
}

#ifndef SKY_CAM
NTSTATUS Cmd_SetResolutionIndex(
    PDEVICE_CONTEXT pDevExt,
    SNSR_MODE Mode,
    ULONG Index
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    DoTraceMessage(FLAG_LOG, "%s Cmd_SetResolutionIndex+ Fmt: index=%d\n", DEVICE_NAME, Index);

    CHECK_POINTER_INVALID(pDevExt)

    if (pDevExt->PowerOnStatus != PWR_UP)
        status = Startup(pDevExt);

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

    status = InitResolution(pDevExt, Mode, Index);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s Cmd_SetResolutionIndex Fail! \n", DEVICE_NAME);
        goto done;
    }

    pDevExt->ResIndex = Index;
    pDevExt->Mode = Mode;
    pDevExt->BankId = 0;
    pDevExt->Binning = (UINT8)Sensor_Res[pDevExt->Mode][pDevExt->ResIndex].Binning;
    pDevExt->MipiBps = (INT32)Sensor_Res[pDevExt->Mode][pDevExt->ResIndex].MipiBps;

done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_SetResolutionIndex index = %d \n", DEVICE_NAME, pDevExt->ResIndex);
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_SetResolutionIndex- (%d) returns with code: 0x%x\n", DEVICE_NAME,
        Index, status);
    return status;
}
#endif

static BOOLEAN IsCHT(UCHAR* pInformation)
{
    CHAR* pInfo = (CHAR*)pInformation;
    
    if (pInfo[0] == 'C' && pInfo[1] == 'H' && pInfo[2] == 'V')
        return TRUE;

    return FALSE;
}

static BOOLEAN IsBYT(UCHAR* pInformation)
{
    CHAR* pInfo = (CHAR*)pInformation;
    
    if (pInfo[0] == 'B' && pInfo[1] == 'Y' && pInfo[2] == 'T')
        return TRUE;

    return FALSE;
}

static BOOLEAN IsBXT(UCHAR* pInformation)
{
    CHAR* pInfo = (CHAR*)pInformation;
    
    if (pInfo[0] == 'B' && pInfo[1] == 'X' && pInfo[2] == 'T')
        return TRUE;

    return FALSE;
}


static BOOLEAN IsELB(UCHAR* pInformation)
{
    CHAR* pInfo = (CHAR*)pInformation;
    
    if (pInfo[7] == 'E' && pInfo[8] == 'L' && pInfo[9] == 'B')
        return TRUE;

    return FALSE;
}


NTSTATUS Cmd_GetSensorInfo(
    PDEVICE_CONTEXT pDevExt,
    PSNSR_INFO pInfo
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(pDevExt)
    CHECK_POINTER(pInfo)
    
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetSensorInfo+\n", DEVICE_NAME);
    pInfo->guid = pDevExt->Guid;
    pInfo->type = (char)pDevExt->SensorMipiConfig.Type;
    pInfo->format = (char)pDevExt->SensorMipiConfig.OutputFormat;


    RtlStringCbCopyA(pInfo->cmos, sizeof(pInfo->cmos), DEVICE_NAME);
    RtlStringCbCopyA(pInfo->module, sizeof(pInfo->module), (CHAR*)pDevExt->DsdtSettings.ModuleName);
    if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_BXT)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "BXT");
        pInfo->mclk_port = pDevExt->MclkPort;
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_BYT)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "BYT");
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_CHT)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "CHT");
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_SKC)
    {
        if (pDevExt->PlatformInfo.hwType == HW_FPGA)
        {
            //used the skc platform to test real sensor with FPGA.
            //When get the platform and test this value, then update this code.
            if (IPU_4P == pDevExt->PlatformInfo.ipuVer)
            {
                RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "CNL");
                pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
            }
            else if (IPU_5 == pDevExt->PlatformInfo.ipuVer)
            {
                RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "LKF");
                pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
            }
            else if (IPU_6 == pDevExt->PlatformInfo.ipuVer)
            {
                RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "TGL");
                pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
            }
        }
        else
        {
            RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "SKY");
            pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
        }
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_CNL)
    {
        if (pDevExt->SensorDriverView.SensorDriverSubView.PlatformSub == PLATFORMSUB_ICL)
        {
            RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "ICL");
        }
        else
        {
            RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "CNL");
        }
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_LKF)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "LKF");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "DEFAULT");
    }

    //might need to specialize below info.
    RtlStringCbCopyA(pInfo->infosub, sizeof(pInfo->infosub), "FFD");
    RtlStringCbCopyA(pInfo->silicon, sizeof(pInfo->silicon), "CHV");
    if (pDevExt->SensorDriverView.SensorDriverSubView.CustomerID == CUSTOMER_NCR)
    {
        RtlStringCbCopyA(pInfo->customer, sizeof(pInfo->customer), "NCR");
    }
    else
    {
        RtlStringCbCopyA(pInfo->customer, sizeof(pInfo->customer), "INTEL");
    }
    RtlStringCbCopyW(pInfo->driverpath, sizeof(pInfo->driverpath), pDevExt->DriverPath.Buffer);

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetSensorInfo- (%s)(%s) returns with code: 0x%x\n", DEVICE_NAME,
        pInfo->module, pInfo->info, status);
done:
    return status;
}


void Cmd_GetMIPIConfigs(
    PDEVICE_CONTEXT pDevExt,
    ULONG *Port,
    ULONG *LaneNum,
    ULONG *OutputFormat
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    UNREFERENCED_PARAMETER(pDevExt);
    
    CHECK_POINTER_INVALID(Port)
    CHECK_POINTER_INVALID(LaneNum)
    CHECK_POINTER_INVALID(OutputFormat)

    *Port = pDevExt->SensorMipiConfig.MipiPort;
    *LaneNum = pDevExt->SensorMipiConfig.MipiLane;
    *OutputFormat = pDevExt->SensorMipiConfig.OutputFormat;

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetMIPIConfigs, port=%d, lanes=%d, format=%d \n", DEVICE_NAME,
        pDevExt->SensorMipiConfig.MipiPort, pDevExt->SensorMipiConfig.MipiLane, pDevExt->SensorMipiConfig.OutputFormat);
    return;
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetMIPIConfigs invalid parameter Port(0x%p), LaneNum(0x%p), OutputFormat(0x%p)\n",
        DEVICE_NAME, Port, LaneNum, OutputFormat);
}

NTSTATUS Cmd_GetCaps (
    PDEVICE_CONTEXT pDevExt,
    SNSR_CAPS **Caps
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(pDevExt)
    CHECK_POINTER(Caps)

    if (!pDevExt->IsEmbeddedDataEnabled)
        pDevExt->SensorCaps.EmbeddedLineNum = 0;

    if (!pDevExt->IsPdafDataEnabled)
        pDevExt->SensorCaps.PDAFLineNum = 0;

    //update Focus capbility accroding to VCM type info read from BIOS
    pDevExt->SensorCaps.FocusSupport = (pDevExt->SsVcm.VcmType == VCM_NONE) ? FALSE : TRUE;
    if (pDevExt->Pld.Panel != PANEL_BACK)
    {
        pDevExt->SensorCaps.PDAFType = 0;
        pDevExt->SensorCaps.PDAFLineNum = 0;
    }

    *Caps = (SNSR_CAPS *)&pDevExt->SensorCaps;
done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetCaps (%Ix) returns with code: 0x%x\n", DEVICE_NAME,  
        (ULONG_PTR)Caps, status);
    return status;
}

NTSTATUS Cmd_GetResolutions (
    PDEVICE_CONTEXT pDevExt,
    SNSR_RESOLUTION_SET **StillRes,
    SNSR_RESOLUTION_SET **VideoRes,
    SNSR_RESOLUTION_SET **PreviewRes,
    SNSR_RESOLUTION_SET **RawRes   
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(pDevExt)
    CHECK_POINTER(StillRes)
    CHECK_POINTER(VideoRes)
    CHECK_POINTER(PreviewRes)

    *StillRes = &ExportResolutionSet[StillMode];
    *VideoRes = &ExportResolutionSet[VideoMode];
    *PreviewRes = &ExportResolutionSet[PreviewMode];
    *RawRes = &ExportResolutionSet[RawMode];

done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetResolutions (%Ix, %Ix, %Ix) returns with code: 0x%x\n", DEVICE_NAME, 
        (ULONG_PTR)StillRes, (ULONG_PTR)VideoRes, (ULONG_PTR)PreviewRes, status);
    return status;
}

NTSTATUS Cmd_GetVersion(
    PDEVICE_CONTEXT pDevExt,
    SNSR_VERSION_INFO *Version
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    
    CHECK_POINTER(pDevExt)
    CHECK_POINTER(Version)
    
    status = Detect(pDevExt, Version);
    if (!NT_SUCCESS(status))
        goto done;

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetVersion Sensor Id = 0x%x\n", DEVICE_NAME, Version->SensorId);
done:
    return status;
}

NTSTATUS Cmd_GetPld(
    IN PDEVICE_CONTEXT pDevExt,
    OUT PACPI_PLD_V2_BUFFER Pld
    )
{
    if (!pDevExt || !Pld)
    {
        return STATUS_INVALID_PARAMETER;
    }

#ifdef SNSR_PLD_WORKAROUND
    UNREFERENCED_PARAMETER(pDevExt);

    memcpy_s(Pld, sizeof(ACPI_PLD_V2_BUFFER), ModulePldInfo, sizeof(ACPI_PLD_V2_BUFFER));
    memcpy_s(&(pDevExt->Pld), sizeof(ACPI_PLD_V2_BUFFER), ModulePldInfo, sizeof(ACPI_PLD_V2_BUFFER));
    
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetPld done pld=%d\n", DEVICE_NAME, pDevExt->Pld.Panel);

    return STATUS_SUCCESS;
#else
    NTSTATUS       status = STATUS_SUCCESS;
    CHAR           pldInfo[PLD_LENGTH];
    ULONG          requiredSize = 0;
    DEVPROPTYPE    type;
    WDFDEVICE      hDevice = pDevExt->DeviceHandle;
    PDEVICE_OBJECT deviceObject = WdfDeviceWdmGetPhysicalDevice(hDevice);
    
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetPld +\n", DEVICE_NAME);

    RtlSecureZeroMemory(pldInfo, PLD_LENGTH);

    status = IoGetDevicePropertyData(
                               deviceObject,
                               &DEVPKEY_Device_PhysicalDeviceLocation,
                               LOCALE_NEUTRAL,
                               0,
                               PLD_LENGTH,
                               pldInfo,
                               &requiredSize,
                               &type
                               );

    if (!(NT_SUCCESS(status)))
    {
        DoTraceMessage(FLAG_ERROR, "IoGetDevicePropertyData returned 0x%x\n", status);
        return status;
    }

    if(requiredSize == PLD_LENGTH)
    {
        memcpy_s(Pld, sizeof(ACPI_PLD_V2_BUFFER), pldInfo, sizeof(ACPI_PLD_V2_BUFFER));
        memcpy_s(&(pDevExt->Pld), sizeof(ACPI_PLD_V2_BUFFER), pldInfo, sizeof(ACPI_PLD_V2_BUFFER));
    }

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetPld - pld=%d\n", DEVICE_NAME, pDevExt->Pld.Panel);
    return status;
#endif
}

// the FPS should be divided by 1000, and we can get the real value.
NTSTATUS Cmd_GetFrameRate(
    PDEVICE_CONTEXT pDevExt,
    PULONG pFps
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG framerate = 0, aestore = 0;

    CHECK_POINTER(pDevExt)
    CHECK_POINTER(pFps)

    //1. fps TBD ...
    framerate = 30000;

    //2. ae store number
    aestore = (pDevExt->SensorCaps.AeFrameStore == 4) ? 4 : 3;

    *pFps = (aestore << 16) | framerate;

    DoTraceMessage(FLAG_LOG, "%s Get AeStore=%d, \n", DEVICE_NAME, aestore);
done:
    return status;
}

NTSTATUS Cmd_GetBinningMode(
    PDEVICE_CONTEXT pDevExt,
    PULONG pBinning
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(pDevExt)
    CHECK_POINTER(pBinning)
    
    *pBinning = pDevExt->Binning;

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetBinningMode binning, buffer pointer = 0x%x,  returns with code: 0x%x\n", 
        DEVICE_NAME, *pBinning, status);
done:
    return status;
}

NTSTATUS Cmd_StartViewfinder(
    PDEVICE_CONTEXT pDevExt,
    ULONG Width,
    ULONG Height
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER_INVALID(pDevExt)

    DoTraceMessage(FLAG_LOG, "%s Cmd_StartViewfinder+\n", DEVICE_NAME);

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

    status = Stream(pDevExt, TRUE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s start viewfinder error! \n", DEVICE_NAME);
        goto done;
    }
    pDevExt->Streaming = TRUE;
done:
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_StartViewfinder- (%d x %d, ) returns with code: 0x%x\n", DEVICE_NAME, 
        Width, Height, status);
    return status;
}

NTSTATUS Cmd_ByteWriteReg(
    PDEVICE_CONTEXT SensorCtx,
    UINT16 Addr,
    UINT16 Data
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(SensorCtx)

    status = RegWrite(SensorCtx, Addr, Data, DATA_8BITS);

done:
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s Cmd_ByteWriteReg failed 0x%x\n", DEVICE_NAME, status);
    }
    else
    {
        DoTraceMessage(FLAG_LOG, "%s Cmd_ByteWriteReg addr 0x%x, data 0x%x\n", DEVICE_NAME, Addr, Data);
    }
    return status;
}

NTSTATUS Cmd_WriteReg(
    PDEVICE_CONTEXT SensorCtx,
    UINT16 Addr,
    UINT16 Data
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(SensorCtx)

#ifdef SENSOR_TYPE_HM2051
    status = RegWrite(SensorCtx, Addr, Data >> 8, DATA_8BITS);
    if (NT_SUCCESS(status))
    {
        status = RegWrite(SensorCtx, ++Addr, Data & 0xFF, DATA_8BITS);
    }
#else
    status = RegWrite(  SensorCtx, Addr, Data,  DATA_16BITS);
#endif

    if (!NT_SUCCESS(status))
        goto done;
done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_WriteRegWord addr 0x%x, data 0x%x\n", DEVICE_NAME, Addr, Data);
    return status;
}

NTSTATUS Cmd_ByteReadReg(
    PDEVICE_CONTEXT SensorCtx,
    UINT16 Addr,
    UINT16* pData
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG data = 0;

    CHECK_POINTER(SensorCtx)
    CHECK_POINTER(pData)

    status = RegRead(SensorCtx, Addr, &data, DATA_8BITS);
done:
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s Cmd_ReadRegWord failed 0x%x\n", DEVICE_NAME, status);
    }
    else
    {
        *pData = (UINT16)data;
        DoTraceMessage(FLAG_LOG, "%s Cmd_ReadRegWord addr 0x%x, data 0x%x\n", DEVICE_NAME, Addr, *pData);
    }   
    return status;
}

NTSTATUS Cmd_ReadReg(
    PDEVICE_CONTEXT SensorCtx,
    UINT16 Addr,
    UINT16* pData
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG data = 0;

    CHECK_POINTER(SensorCtx)
    CHECK_POINTER(pData)

#ifdef SENSOR_TYPE_HM2051
    status = RegRead(  SensorCtx, Addr, &data,  DATA_8BITS);
    if (NT_SUCCESS(status))
    {
        *pData = (UINT16)data << 8;

        status = RegRead(SensorCtx, ++Addr, &data, DATA_8BITS);
        if (NT_SUCCESS(status))
        {
            *pData += (UINT16)data;
        }
    }

    if (!NT_SUCCESS(status))
        goto done;

#elif defined (MIPI_CSI_BRIDGE)
    if (Addr >= 0x0104)
    {
        // In BIOS, Bridge chip I2C device type is <VCM>
        status = RegReadExt(SensorCtx, Addr, &data, DATA_32BITS, ADDR_16BITS, I2C_VCM);
        ConvertULONGForI2C(SensorCtx, (UINT32*)(&data));
    }
    else
        status = RegReadExt(SensorCtx, Addr, &data, DATA_16BITS, ADDR_16BITS, I2C_VCM);
#else
    status = RegRead(  SensorCtx, Addr, &data,  DATA_16BITS);

    if (!NT_SUCCESS(status))
        goto done;
    *pData = (UINT16)data;
#endif

done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_ReadRegWord addr 0x%x, data 0x%x\n", DEVICE_NAME, Addr, data);
    return status;
}


NTSTATUS Cmd_StopViewfinder(
    PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER_INVALID(pDevExt)

    DoTraceMessage(FLAG_LOG, "%s Cmd_StopViewfinder+\n", DEVICE_NAME);

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

    // Viewfinder off
    pDevExt->Streaming = FALSE;
    status = Stream(pDevExt, FALSE);
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s stop viewfinder error! begin to shutdown!\n", DEVICE_NAME);
        status = Shutdown(pDevExt);
    }
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_StopViewfinder- returns with code: 0x%x\n", DEVICE_NAME, status);
    return status;
}

NTSTATUS Cmd_GetExtFeature(
    PDEVICE_CONTEXT pDevExt,
    SNSR_EXT_FEATURE_ID feature,
    PULONG pValue
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(pDevExt)
    CHECK_POINTER(pValue)

    switch (feature)
    {
    case SNSR_EXT_FEATURE_MIPICLK:
        //mipi bit rate per lane
        *pValue = pDevExt->MipiBps;
        DoTraceMessage(FLAG_LOG, "%s Cmd_GetExtFeature Mipi Clock = %d,  returns with code: 0x%x\n", DEVICE_NAME, *pValue, status);
        break;
    default:
        status = STATUS_NOT_SUPPORTED;
        break;
    }
done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetExtFeature (ID = %d) returns with code: 0x%x\n", DEVICE_NAME, feature, status);
    return status;
}

NTSTATUS Cmd_SetExtFeature(
    PDEVICE_CONTEXT pDevExt,
    SNSR_EXT_FEATURE_ID feature,
    ULONG_PTR Value
    )
{
    NTSTATUS status = STATUS_NOT_SUPPORTED;

    UNREFERENCED_PARAMETER(pDevExt);
    UNREFERENCED_PARAMETER(feature);
    UNREFERENCED_PARAMETER(Value);

    switch (feature)
    {
#ifdef AE_EXACT_CONFIG
    case SNSR_EXT_FEATURE_EOF:

        pDevExt->N_exp_id = *((LONG*)Value);
        {
            LARGE_INTEGER dueTime;
            INT32 indexExp, indexGain, indexDGain;

            // Start Mutex
            status = KeWaitForSingleObject(&pDevExt->ExpEvent, Executive, KernelMode, FALSE, NULL);
            if (status != STATUS_SUCCESS)
            {
                DoTraceMessage(FLAG_ERROR, "%s SNSR_EXT_FEATURE_EOF KeWaitForSingleObject fail, (status 0x%x)\n",
                    DEVICE_NAME, status);
                KeSetEvent(&pDevExt->ExpEvent, IO_NO_INCREMENT, FALSE);
                break;
            }

            DoTraceMessage(FLAG_LOG, "%s Cmd_SetExtFeature SNSR_EXT_FEATURE_EOF, During Frame=%d\n", DEVICE_NAME, pDevExt->N_exp_id);
            indexExp = expIndex(pDevExt->N_Frame + pDevExt->ExpEffectDelay);
            indexGain = expIndex(pDevExt->N_Frame + pDevExt->AGainEffectDelay);
            indexDGain = expIndex(pDevExt->N_Frame + pDevExt->DGainEffectDelay);

            if (pDevExt->ExpTab[indexExp].ExpStatus == EXPSTS_UPDATED)
            {
                pDevExt->ExpFetch.Exp.CoarseIntegrationTime = pDevExt->ExpTab[indexExp].Exp.CoarseIntegrationTime;
                pDevExt->ExpFetch.Exp.FineIntegrationTime = pDevExt->ExpTab[indexExp].Exp.FineIntegrationTime;
                pDevExt->ExpFetch.Exp.FrameLengthLines = pDevExt->ExpTab[indexExp].Exp.FrameLengthLines;
                pDevExt->ExpFetch.Exp.LineLengthPixels = pDevExt->ExpTab[indexExp].Exp.LineLengthPixels;
                pDevExt->ExpFetch.ExpStatus = EXPSTS_FETCHED;
                pDevExt->ExpTab[indexExp].ExpStatus = EXPSTS_FETCHED;
                DoTraceMessage(FLAG_LOG, "%s Fetched Frame %d ExposureTime", DEVICE_NAME, pDevExt->N_exp_id + pDevExt->ExpEffectDelay);
            }
            else
            {
                pDevExt->ExpFetch.ExpStatus = EXPSTS_INVALID;
                DoTraceMessage(FLAG_LOG, "%s UnFetched ExposureTime ", DEVICE_NAME);
            }

            if (pDevExt->ExpTab[indexGain].GainStatus == EXPSTS_UPDATED)
            {
                pDevExt->ExpFetch.Exp.AnalogGain = pDevExt->ExpTab[indexGain].Exp.AnalogGain;
                pDevExt->ExpFetch.GainStatus = EXPSTS_FETCHED;
                pDevExt->ExpTab[indexGain].GainStatus = EXPSTS_FETCHED;
                DoTraceMessage(FLAG_LOG, "%s Fetched Frame %d Gain ", DEVICE_NAME, pDevExt->N_exp_id + pDevExt->AGainEffectDelay);
            }
            else
            {
                pDevExt->ExpFetch.GainStatus = EXPSTS_INVALID;
                DoTraceMessage(FLAG_LOG, "%s UnFetched Gain ", DEVICE_NAME);
            }

            if (pDevExt->ExpTab[indexDGain].DGainStatus == EXPSTS_UPDATED)
            {
                pDevExt->ExpFetch.Exp.DigitalGain = pDevExt->ExpTab[indexDGain].Exp.DigitalGain;
                pDevExt->ExpFetch.DGainStatus = EXPSTS_FETCHED;
                pDevExt->ExpTab[indexDGain].DGainStatus = EXPSTS_FETCHED;
                DoTraceMessage(FLAG_LOG, "%s Fetched Frame %d Digital Gain ", DEVICE_NAME, pDevExt->N_exp_id + pDevExt->DGainEffectDelay);
            }
            else
            {
                pDevExt->ExpFetch.DGainStatus = EXPSTS_INVALID;
                DoTraceMessage(FLAG_LOG, "%s UnFetched Digital Gain ", DEVICE_NAME);
            }

            if (   pDevExt->ExpTab[indexExp].ExpStatus == EXPSTS_FETCHED 
                || pDevExt->ExpFetch.GainStatus == EXPSTS_FETCHED
                || pDevExt->ExpFetch.DGainStatus == EXPSTS_FETCHED)
            {
                INT32 index = expIndex(pDevExt->N_Frame + 1);
                LONG FrameLengthLines, FLines;

                if (pDevExt->ExpTab[index].ExpStatus == EXPSTS_INVALID)
                {
                    DoTraceMessage(FLAG_LOG, "Calc delay: Use Default, invalid status\n");
                    pDevExt->remainTimeExp = pDevExt->DefaultDelay;
                }
                else
                {
                    FrameLengthLines = (LONG)(pDevExt->ExpTab[index].Exp.FrameLengthLines);
                    FLines = pDevExt->DefaultVTS;
                    if (FLines >= (LONG)(pDevExt->ExpTab[index].Exp.CoarseIntegrationTime + 4))
                    {
                        DoTraceMessage(FLAG_LOG, "Calc delay: Use Default, normal status\n");
                        pDevExt->remainTimeExp = pDevExt->DefaultDelay;
                    }
                    else
                    {
                        LONG deltaTime;
                        DoTraceMessage(FLAG_LOG, "Calc delay: Use Default, large exposure status\n");
                        FLines = (LONG)(pDevExt->ExpTab[index].Exp.CoarseIntegrationTime + 4);
                        if (FLines < FrameLengthLines)
                            FLines = FrameLengthLines;
                        FLines = (FLines / 2 + 1) * 2;

                        deltaTime = (FLines - pDevExt->DefaultVTS)*pDevExt->DefaultHTS/(pDevExt->CurPixClkFreqHz/1000);

                        pDevExt->remainTimeExp = pDevExt->DefaultDelay + deltaTime;
                    }
                }
                pDevExt->preTimeExp = (LONG)QuerySystemTime();
                pDevExt->remainTimeExp += 2; //ms
                DoTraceMessage(FLAG_LOG, "Start TimerExposure, cur=%dms, remain=%dms \n", pDevExt->preTimeExp, pDevExt->remainTimeExp);
                dueTime.QuadPart = pDevExt->remainTimeExp * (-10000);
                KeSetTimer(&(pDevExt->TimerExposure), dueTime, &(pDevExt->DPCExposure));
            }

            pDevExt->ExpTab[expIndex(pDevExt->N_Frame + 1)].ExpStatus = EXPSTS_INVALID;
            pDevExt->ExpTab[expIndex(pDevExt->N_Frame + 1)].GainStatus = EXPSTS_INVALID;
            pDevExt->ExpTab[expIndex(pDevExt->N_Frame + 1)].DGainStatus = EXPSTS_INVALID;

            pDevExt->N_Frame++;

            status = expReadYmean(pDevExt, &(pDevExt->CurYmean));

            // End Mutex
            KeSetEvent(&pDevExt->ExpEvent, IO_NO_INCREMENT, FALSE);

            break;
        }
#endif
    default:
        status = STATUS_NOT_SUPPORTED;
        break;
    }

    return status;
}

NTSTATUS Cmd_SetPlatformInfo(PDEVICE_CONTEXT pDevExt, PPLAT_INFO PlatformInfo)
{
    NTSTATUS status = STATUS_SUCCESS;

    if (NULL == PlatformInfo)
    {
        return STATUS_INVALID_PARAMETER;
    }

    pDevExt->PlatformInfo = *PlatformInfo;
    return status;
}

NTSTATUS Cmd_SetPos(
    PDEVICE_CONTEXT pDevExt,
    USHORT FocusPos
    )
{
    DoTraceMessage(FLAG_LOG, "%s Cmd_SetPos+ ", DEVICE_NAME);
    if (!pDevExt->SsVcm.SetFocusPos)
        return STATUS_NOT_SUPPORTED;

    return pDevExt->SsVcm.SetFocusPos(&pDevExt->SsVcm, FocusPos);
}

NTSTATUS Cmd_GetPos(
    PDEVICE_CONTEXT pDevExt,
    USHORT *FocusPos
    )
{
    if (!pDevExt->SsVcm.GetFocusPos)
        return STATUS_NOT_SUPPORTED;

    return pDevExt->SsVcm.GetFocusPos(&pDevExt->SsVcm, FocusPos);
}

NTSTATUS Cmd_GetStatus(
    PDEVICE_CONTEXT pDevExt,
    PULONG Status    
    )
{
    if (!pDevExt->SsVcm.GetFocusStatus)
        return STATUS_NOT_SUPPORTED;

    return pDevExt->SsVcm.GetFocusStatus(&pDevExt->SsVcm, Status);
}

NTSTATUS Cmd_GetHPStatus(
    PDEVICE_CONTEXT pDevExt,
    PULONG Status    
    )
{
    if (!pDevExt->SsVcm.GetFocusHPStatus)
        return STATUS_NOT_SUPPORTED;

    return pDevExt->SsVcm.GetFocusHPStatus(&pDevExt->SsVcm, Status);
}

NTSTATUS Cmd_SetConfig(
    PDEVICE_CONTEXT pDevExt,
    SNSR_VCMDATA* pSnsrVcmData
    )
{
    if (!pDevExt->SsVcm.SetVcmConfiguration)
        return STATUS_NOT_SUPPORTED;

    return pDevExt->SsVcm.SetVcmConfiguration(&pDevExt->SsVcm, pSnsrVcmData);
}


NTSTATUS Cmd_GetNvmData(
    PDEVICE_CONTEXT pDevExt,
    PULONG Size,
    PUINT8 Buffer
    )
{
    if (!pDevExt->SsNvm.GetCalibrationData)
        return STATUS_NOT_SUPPORTED;

    pDevExt->SsNvm.IsLoadingStage = (pDevExt->DriverStage == DRIVER_STAGE_LOADING) ? TRUE : FALSE;

    return pDevExt->SsNvm.GetCalibrationData(&pDevExt->SsNvm, Size, Buffer);
}

NTSTATUS Cmd_NvmWrite(
    PDEVICE_CONTEXT pDevExt,
    UINT16 Addr,
    UINT8* pData,
    INT32 Length
    )
{
    if (!pDevExt->SsNvm.SetEEPRom)
        return STATUS_NOT_SUPPORTED;

    return pDevExt->SsNvm.SetEEPRom(&pDevExt->SsNvm, Addr, pData, Length);
}

NTSTATUS Cmd_NvmRead(
    PDEVICE_CONTEXT pDevExt,
    UINT16 Addr,
    UINT8* pData,
    INT32 Length
    )
{
    if (!pDevExt->SsNvm.GetEEPRom)
        return STATUS_NOT_SUPPORTED;

    return pDevExt->SsNvm.GetEEPRom(&pDevExt->SsNvm, Addr, pData, Length);
}

#if defined(OTM_SENSOR_CONTROL_LOGIC) || defined(SKY_CAM)

NTSTATUS QueryControlLogicDeviceInterface(PDEVICE_OBJECT deviceObject, CONTROLLOGIC_SENSOR_INTERFACE* intf)
{
    PAGED_CODE();
    PIRP irp;
    IO_STATUS_BLOCK ioStatus;
    KEVENT event;
    NTSTATUS status;
    PIO_STACK_LOCATION irpStack = NULL;

    KeInitializeEvent(&event, NotificationEvent, FALSE);
    irp = IoBuildSynchronousFsdRequest(IRP_MJ_PNP,
        deviceObject,
        NULL,
        0,
        NULL,
        &event,
        &ioStatus
        );

    if (irp == NULL)
        return STATUS_INSUFFICIENT_RESOURCES;

    irp->IoStatus.Status = STATUS_NOT_SUPPORTED;
    irpStack = IoGetNextIrpStackLocation(irp);

    RtlSecureZeroMemory(irpStack, sizeof(IO_STACK_LOCATION));
    irpStack->MajorFunction = IRP_MJ_PNP;
    irpStack->MinorFunction = IRP_MN_QUERY_INTERFACE;
    irpStack->Parameters.QueryInterface.InterfaceType = &ControlLogicSensorInterfaceStandard;
    irpStack->Parameters.QueryInterface.Interface = (PINTERFACE)intf;
    irpStack->Parameters.QueryInterface.Size = sizeof(*intf);
    irpStack->Parameters.QueryInterface.Version = CONTROLLOGIC_SENSOR_INTERFACE_VERSION;

    status = IoCallDriver(deviceObject, irp);
    if (status == STATUS_PENDING)
    {
        status = KeWaitForSingleObject(&event, Executive, KernelMode,
            FALSE, NULL);
        ASSERT(NT_SUCCESS(status));
        status = ioStatus.Status;
    }

    if (NT_SUCCESS(status))
    {
        if (!intf)
            return STATUS_INVALID_PARAMETER;

        if (intf->InterfaceHeader.Version != CONTROLLOGIC_SENSOR_INTERFACE_VERSION
            || (intf->InterfaceHeader.Size != sizeof(CONTROLLOGIC_SENSOR_INTERFACE)))
        {
            status = STATUS_UNSUCCESSFUL;
            DoTraceMessage(FLAG_ERROR, "Invalid ControlLogic interface, ver:%d size:%d",
                intf->InterfaceHeader.Version, intf->InterfaceHeader.Size);
        }
        else
        {
            DoTraceMessage(FLAG_LOG, "ControlLogic interface ver:%d size:%d",
                intf->InterfaceHeader.Version, intf->InterfaceHeader.Size);
        }
    }

    return status;
}

NTSTATUS  CleanUpSensorInterfaceContext(
    PDEVICE_CONTEXT deviceCtx)
{
    NTSTATUS status = STATUS_SUCCESS;
    NT_ASSERT(deviceCtx);

    //DoTraceMessage(FLAG_LOG, "%s CleanUpSensorInterfaceContext-- in", DEVICE_NAME);

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

    if (deviceCtx->ControlLogicInterface)
    {
        ExFreePoolWithTag(deviceCtx->ControlLogicInterface, POOL_TAG);
        deviceCtx->ControlLogicInterface = NULL;
    }

    if (deviceCtx->ControlLogicDeviceObject)
    {
        ObDereferenceObject(deviceCtx->ControlLogicDeviceObject);
        deviceCtx->ControlLogicDeviceObject = NULL;
    }

    KeSetEvent(&deviceCtx->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, "%s CleanUpSensorInterfaceContext returns with code: 0x%x\n",
        SENSOR_NAME, status);

    return status;

}
#endif

#ifdef SKY_CAM
NTSTATUS Cmd_GetControlLogicInfo(
    PDEVICE_CONTEXT deviceCtx,
    PVOID info
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    NT_ASSERT(deviceCtx);
    NT_ASSERT(info);

    if (!deviceCtx ||
        !info)
    {
        status = STATUS_FWP_NULL_POINTER;
        return status;
    }

    memcpy_s(info, sizeof(deviceCtx->ControlLogicInfo), &deviceCtx->ControlLogicInfo, sizeof(deviceCtx->ControlLogicInfo));

    DoTraceMessage(FLAG_LOG, "%s BoardType 0x%x, ControlLogicID 0x%x, PmicType 0x%x\n",
        SENSOR_NAME,
        deviceCtx->ControlLogicInfo.BoardType,
        deviceCtx->ControlLogicInfo.ControlLogicID,
        deviceCtx->ControlLogicInfo.PmicType);

    return status;
}
#endif

void TraceWPPInit(PDRIVER_OBJECT pDriverObj, PUNICODE_STRING pUnicodeStr)
{
    WPP_INIT_TRACING(pDriverObj, pUnicodeStr);
}

void TraceWPPUninit(PDRIVER_OBJECT pDriverObj)
{
    WPP_CLEANUP(pDriverObj);
}

/**
   workaround of wpp output.
**/
void TraceWPPLogFn(tag_sensor_trace_level level, const PCHAR fmt, ...)
{
    va_list args;
    char szBuf[1024] = {0};

    va_start(args, fmt);
    vsprintf_s(szBuf, sizeof(szBuf), fmt, args);
    va_end(args);

    if (FLAG_LOG == level)
    {
        DoTraceMessage(FLAG_LOG, "%s", szBuf);
    }
    else if (FLAG_WARN == level)
    {
        DoTraceMessage(FLAG_WARN, "%s", szBuf);
    }
    else if (FLAG_ERROR == level)
    {
        DoTraceMessage(FLAG_ERROR, "%s", szBuf);
    }
}
