/*************************************************************************************************
// INTEL CONFIDENTIAL Copyright 2019-2026 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"
#ifdef I2C_QUEUE_BIG_BUF
/* VD55G0/1 FW size 6868 byte */
#define MAX_BUF (6868+2)
#else
#define MAX_BUF (32+2)
#endif

INT32 g_GuidMask = 0;
INT32 g_MaxInstanceNum = 1;

#define CHECK_DEVICE_HANDLE(input) { \
    if (!input) \
    { \
         return STATUS_INVALID_HANDLE; \
    } \
}

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;
}

ULONG ConvertToRealOrder(ULONG Data, ULONG Length)
{
    const char *SensorName = DEVICE_NAME;
    if (_stricmp(SensorName, "VD55G0") != 0 && _stricmp(SensorName, "VD55G1") != 0 &&
        _stricmp(SensorName, "VD25G1") != 0 && _stricmp(SensorName, "VD25G4") != 0 &&
        _stricmp(SensorName, "TBE0020A1") != 0)
    {
        return Data;
    }

    UINT8* p = (UINT8*)&Data;
    int from = 0, to = Length - 1;
    while (from < to)
    {
        UINT8 tmp = p[from];
        p[from] = p[to];
        p[to] = tmp;
        ++from;
        --to;
    }
    return Data;
}

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

    CHECK_POINTER(SensorCtx)
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)

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

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

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

    CHECK_POINTER(SensorCtx)
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)

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

    Data = ConvertToRealOrder(Data, DataBits);
    status = Commoni2cWrite(&SensorCtx->I2cDev, Addr, Data, DataBits, index);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s write 0x%x <- 0x%x try again!! (0x%x)", DEVICE_NAME, Addr, Data, status);
        SleepMSecond(2);
        status = Commoni2cWrite(&SensorCtx->I2cDev, Addr, Data, DataBits, index);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s write 0x%x <- 0x%x fail!! (0x%x)", DEVICE_NAME, Addr, Data, status);
            goto done;
        }
    }
    if (DataBits == DATA_8BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%04x -> 0x%02x (0x%x)", DEVICE_NAME, Addr, Data, status);
    else if (DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%04x -> 0x%04x (0x%x)", DEVICE_NAME, Addr, Data, status);
    else if (DataBits == DATA_24BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%04x -> 0x%06x (0x%x)", 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)
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)

    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, "[ERROR] %s write 0x%x <- 0x%x try again!! (0x%x)", DEVICE_NAME, Addr, Data, status);
        SleepMSecond(2);
        status = i2cWriteExt(&SensorCtx->I2cDev, Addr, Data, DataBits, AddrBits, I2CIndex);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s write 0x%x <- 0x%x fail!! (0x%x)", 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)", DEVICE_NAME, Addr, Data, status);

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

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

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

    if (AddrBits == ADDR_32BITS && DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s write 0x%08x -> 0x%04x (0x%x)", 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;
    }
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)

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

    *Data = ConvertToRealOrder(*Data, DataBits);

    if (DataBits == DATA_8BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%02x (0x%x)", DEVICE_NAME, Addr, *Data, status);
    else if (DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%04x (0x%x)", DEVICE_NAME, Addr, *Data, status);
    else if (DataBits == DATA_24BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%06x (0x%x)", DEVICE_NAME, Addr, *Data, status);
    else if (DataBits == DATA_32BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%04x -> 0x%08x (0x%x)", 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;
    }
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)

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

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

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

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

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

    if (AddrBits == ADDR_32BITS && DataBits == DATA_16BITS)
        DoTraceMessage(FLAG_LOG, "%s read 0x%08x -> 0x%04x (0x%x)", 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)
    {
        return STATUS_INVALID_PARAMETER;
    }
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)

    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_DEVICE_HANDLE(SensorCtx->DeviceHandle)
    CHECK_POINTER(regs)

    while(p->Type != DATA_TOKE_TERM)
    {
        if (p->Type >= DATA_8BITS && p->Type <= DATA_32BITS)
        {
            status = RegWrite(SensorCtx, (UINT16)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_DEVICE_HANDLE(SensorCtx->DeviceHandle)
    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 if (AddrBits == ADDR_16BITS)
            {
                wbuf[index++] = (UINT8)(p->Reg >> 8);
                wbuf[index++] = (UINT8)p->Reg;
            }
            else if (AddrBits == ADDR_24BITS)
            {
                wbuf[index++] = (UINT8)(p->Reg >> 16);
                wbuf[index++] = (UINT8)(p->Reg >> 8);
                wbuf[index++] = (UINT8)p->Reg;
            }
            else if (AddrBits == 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;
            }

            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, "[ERROR] %s Failed to write reg 0x%x", 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, "[ERROR] %s Failed to write reg 0x%x", 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_DEVICE_HANDLE(SensorCtx->DeviceHandle)
    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, "[ERROR] %s Failed to write reg 0x%x", DEVICE_NAME, status);
                goto done;
            }
        }

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

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


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;
    }
    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].BayerOrder = 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}", DEVICE_NAME,
            pTable[index].Width, pTable[index].Height,
            pTable[index].CropX, pTable[index].CropY,
            pTable[index].FPS, pTable[index].BayerOrder,
            pTable[index].IsFullFov);
    }
}

NTSTATUS SetAEToCVChip(PDEVICE_CONTEXT SensorCtx, AE_SETTING Aesetting)
{
    NTSTATUS status = STATUS_SUCCESS;
    // Send AE setting(ET, a-gain, d-gain) configuration to CV chip through camera<->vision driver DDI
    if (SensorCtx->CVFInterface && ((PINTERFACE)(SensorCtx->CVFInterface))->Version > VISION_INTERFACE_VERSION_12)
    {
        status = ((PXSENSOR_DRV_INTERFACE)(SensorCtx->CVFInterface))
                    ->control(((PINTERFACE)(SensorCtx->CVFInterface))->Context, XSD_AESETTING_CFG, &Aesetting);
        goto done;
    }
    else
    {
        status = STATUS_NOT_SUPPORTED;
        goto done;
    }

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

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;
    }
    CHECK_DEVICE_HANDLE(deviceCtx->DeviceHandle)

    for (mode = 0; mode < maxMode; mode ++)
    {
        for (i = 0; i <= RES_BAYER_MAX; i ++)
        {
            if (Sensor_Res[mode][i].regs == NULL)
                break;
        }
        ResNum = i;
        if (ResNum > MaxNum[mode])
        {
            DoTraceMessage(FLAG_LOG, "%s SsInitExportResTable Fail, %s mode count %d overrun", DEVICE_NAME, PrintString[mode], ResNum);
            return STATUS_UNSUCCESSFUL;
        }

        ExportResolutionSet[mode].Count = ResNum;
        ExportResolutionSet[mode].Resolutions = ResExport[mode];
        DoTraceMessage(FLAG_LOG, "%s %s %d Resolutions", 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)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)

    if (pDevExt->N_Frame < -3)
    {
        DoTraceMessage(FLAG_ERROR, "[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, "[ERROR] %s aeCmd_SetExposure KeWaitForSingleObject fail, (status 0x%x)",
            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 = LocalCmdSetExposure(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", 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");
    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 ", 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
    )
{
    if (!deviceCtx)
    {
        DoTraceMessage(FLAG_LOG, "ERROR! deviceCtx %p", deviceCtx);
        return;
    }

    if (!deviceCtx->DeviceHandle)
    {
        DoTraceMessage(FLAG_LOG, "ERROR! deviceCtx %p", deviceCtx);
        return;
    }

    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;
    }

    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)

    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, "[ERROR] %s Failed to set standby pin active 0x%x", 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, "[ERROR] %s Failed to set Power 0 pin active 0x%x", 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, "[ERROR] %s Failed to set Power 1 pin active 0x%x", 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, "[ERROR] %s Failed to set standby pin active 0x%x", 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, "[ERROR] %s Failed to set PWDN pin active 0x%x", 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, "[ERROR] %s Failed to set RESET pin 0x%x", 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, "[ERROR] %s Failed to set PWDN pin active 0x%x", 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, "[ERROR] %s Failed to set RESET pin 0x%x", 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, "[ERROR] %s Failed to set PWDN pin inactive 0x%x", 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, "[ERROR] %s Failed to set Power 1 pin active 0x%x", 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, "[ERROR] %s Failed to set Power 0 pin active 0x%x", DEVICE_NAME, status);
                return status;
            }
            SleepMSecond(DELAY_PWDN);
        }
    }

    return status;
}

static void LoadModuleInfo(IN PDEVICE_CONTEXT pDevExt)
{
    if (!pDevExt || !pDevExt->DeviceHandle)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s LoadModuleInfo pdev is NULL", DEVICE_NAME);
        return;
    }
    if (pDevExt->SsNvm.ModuleInfo.MagicNumber[0] == 'W' &&
        pDevExt->SsNvm.ModuleInfo.MagicNumber[1] == 'D' &&
        pDevExt->SsNvm.ModuleInfo.MagicNumber[2] == 'O' &&
        pDevExt->SsNvm.ModuleInfo.MagicNumber[3] == 'S')
    {
        pDevExt->SsNvm.IsModuleInfoAvailable = TRUE;
        DoTraceMessage(FLAG_LOG, "%s LoadModuleInfo: Module Info is available in NVM ", DEVICE_NAME);
    }
    else
    {
        pDevExt->SsNvm.IsModuleInfoAvailable = FALSE;
        DoTraceMessage(FLAG_LOG, "%s LoadModuleInfo: No Module Info in NVM ", DEVICE_NAME);
    }
}

static NTSTATUS WriteModuleInfoToDsdt(IN PDEVICE_CONTEXT pDevExt)
{
    NTSTATUS status = STATUS_SUCCESS;
    if (!pDevExt || !pDevExt->DeviceHandle)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s WriteModuleInfoToDsdt pdev is NULL", DEVICE_NAME);
        return STATUS_INVALID_HANDLE;
    }

    ULONG ModuleNameIndex = pDevExt->SsNvm.ModuleInfoFromOTP.bits.Module_Name_Index;
    if (ModuleNameIndex >= MAX_MODULE_NUM)
    {
        DoTraceMessage(FLAG_LOG, "%s Index is out of range %d", DEVICE_NAME, ModuleNameIndex);
        status = STATUS_UNSUCCESSFUL;
        goto done;
    }
    status = RtlStringCbCopyA((CHAR*)&pDevExt->DsdtSettings.ModuleName[0], STRING_LENGTH, (CHAR*)pDevExt->ModuleNameTable[ModuleNameIndex].ModuleName);
    if (NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s Get DSM Sensor Module %s", DEVICE_NAME, (CHAR*)pDevExt->DsdtSettings.ModuleName);
    }
done:
    return status;
}

static NTSTATUS GetModuleInfoFromOTP(
    IN PDEVICE_CONTEXT pDevExt
)
{
    NTSTATUS status = STATUS_SUCCESS;
    if (!pDevExt)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s GetModuleInfoFromOTP pdev is NULL", DEVICE_NAME);
        return STATUS_INVALID_HANDLE;
    }
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    // if has module info from otp, validate module info
    if (pDevExt->SsNvm.HasModuleInfoFromOTP)
    {
        Module_Data ModuleInfoFromOTP = pDevExt->SsNvm.ModuleInfoFromOTP;
        UINT16 orig_checksum = 0;
        UINT16 real_checksum = 0;
        real_checksum = Crc16((UINT8*)&ModuleInfoFromOTP, 2);
        orig_checksum = (UINT16)ModuleInfoFromOTP.bits.Check_sum;
        if (real_checksum != orig_checksum)
        {
            status = STATUS_UNSUCCESSFUL;
            DoTraceMessage(FLAG_ERROR, "Calibration Checksum failed : orig(0x%x), real(0x%x)\n", orig_checksum, real_checksum);
            goto done;
        }
        status = WriteModuleInfoToDsdt(pDevExt);
    }
done:
    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;
    }
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)

    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, "[ERROR] %s GetAcpiSsdbData failed with status 0x%x", 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",
                                    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);
        }

    }
    status = GetAcpiDsmData(pDevExt->DeviceHandle,
                            POOL_TAG, (PUCHAR)&DsmInterfaceVersion, 0, 1, 0, sizeof(ULONG), &value);
    if (NT_SUCCESS(status))
    {
        pDevExt->DsdtSettings.InterfaceVersion = value;
    }

    // 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;
    }

    DoTraceMessage(FLAG_LOG, "%s Get DSM Sensor Module %s", DEVICE_NAME, (CHAR *)pDevExt->DsdtSettings.ModuleName);

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

    // VCM type
    //WA for IPU6 SS Cosmos as the RVP bios menu do not support the new VCM VCM_DW9808
    if ((pDevExt->SensorDriverView.SensorDriverSubView.VcmType == VCM_AK7371) && (_stricmp(DEVICE_NAME, "S5K3L6XX04") == 0))
    {
        pDevExt->SensorDriverView.SensorDriverSubView.VcmType = VCM_DW9808;
    }

    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", DEVICE_NAME, pDevExt->SsVcm.VcmType);

    // Get VcmIndex by VcmType from Vcm function pointer table
    pDevExt->SsVcm.VcmIndex = GetIndexFromVcmTable(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 = NVM_CAT24C64;
    }
    //WA for IPU6 SS Cosmos as the RVP bios menu do not support the new NVM_DW9808
    //config 0x0A: ROM_EFLASH_DW9806B in BIOS, mock to NVM_DW9808
    if ((pDevExt->SensorDriverView.SensorDriverSubView.ROMType == 0x0A) && (_stricmp(DEVICE_NAME, "S5K3L6XX04") == 0))
    {
        pDevExt->SensorDriverView.SensorDriverSubView.ROMType = NVM_DW9808;
    }
    //WA for sensor IMX681 as we don't have this NVM type in BIOS
    if ((pDevExt->SensorDriverView.SensorDriverSubView.ROMType != 0) &&
        ((_stricmp((CHAR *)pDevExt->DsdtSettings.ModuleName, "MSHW0520") == 0)
            || (_stricmp((CHAR *)pDevExt->DsdtSettings.ModuleName, "MSHW0580") == 0)
            || (_stricmp((CHAR *)pDevExt->DsdtSettings.ModuleName, "MSHW0740") == 0)))
    {
        pDevExt->SensorDriverView.SensorDriverSubView.ROMType = NVM_GT24P128F;
    }
    pDevExt->DsdtSettings.RomType = pDevExt->SensorDriverView.SensorDriverSubView.ROMType;
    DoTraceMessage(FLAG_LOG, "%s Get ROM type %d", 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;
        //set NVRAM Device Handle as parent(sensor) device handle.
        pDevExt->SsNvm.DevHandle = pDevExt->DeviceHandle;
        DoTraceMessage(FLAG_LOG, "%s Sensor RomType is %d", 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, "[ERROR] %s Get DSM calibration data failed with status:0x%x", DEVICE_NAME, status);
            RtlStringCbCopyA((CHAR*)pSsNvm->NvmBuffer, 8, "UNKNOWN");
            if (status == STATUS_BUFFER_OVERFLOW)
                DoTraceMessage(FLAG_ERROR, "[ERROR] %s Sensor Calibration Data in BIOS can not be more than %d", 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_3K_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", 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;
    }
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)

    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, Corresponded flash device shall be with flash id %u",
        DEVICE_NAME, flashSupport, SensorCtx->SensorDriverView.SensorDriverSubView.FlashId);

    // 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;
    SensorCtx->SensorMipiExt.OutputFormat = DefaultSensorMipiConfig.OutputFormat;
    SensorCtx->SensorMipiExt.Type = DefaultSensorMipiConfig.Type;
    // mipi extend
    if (SensorCtx->SensorDriverView.SensorDriverSubView.MipiDefine == MIPI_IN_ACPI)
    {
        SensorCtx->SensorMipiExt.LaneConfiguration = SensorCtx->SensorDriverView.SensorDriverSubView.LaneConfiguration;
        SensorCtx->SensorMipiExt.LaneNum = SensorCtx->SensorDriverView.LanesUsed;
        SensorCtx->SensorMipiExt.PhyConfig = SensorCtx->SensorDriverView.SensorDriverSubView.PhyConfig;
        SensorCtx->SensorMipiExt.Port = SensorCtx->SensorDriverView.LinkUsed;
    }

    // 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);

    ULONG value = 0;
    NTSTATUS status = GetAcpiDsmData(SensorCtx->DeviceHandle, POOL_TAG, (PUCHAR)&DsmCameraCVF, 0, 0, 0, sizeof(ULONG), &value);

    if ((status != STATUS_SUCCESS) || !value)
    {
        SensorCtx->SensorCaps.CVFSupport = CVF_SUPPORT_DISABLED;
        DoTraceMessage(FLAG_LOG, "%s BIOS DSM dose not exist", DEVICE_NAME);
    }
    else
    {
        // BIOS DSM exists, Query CVF capability.
        status = GetAcpiDsmData(SensorCtx->DeviceHandle, POOL_TAG, (PUCHAR)&DsmCameraCVF, 0, 1, 0, sizeof(ULONG), &value);

        if (status != STATUS_SUCCESS)
        {
            SensorCtx->SensorCaps.CVFSupport = CVF_SUPPORT_DISABLED;
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s Failed to read CVF configuration, disabling it", DEVICE_NAME);
        }
        else
        {
            SensorCtx->SensorCaps.CVFSupport = value > CVF_SUPPORT_DISABLED ? TRUE : FALSE;
            DoTraceMessage(FLAG_LOG, "%s CVF configuration is %d", DEVICE_NAME, value);
        }
    }

    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
    )
{
    if (!pDevExt || !pDevExt->DeviceHandle)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s InitInterfaceVcmNvm pdev is NULL", DEVICE_NAME);
        return;
    }

    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;
}

static void InitPlatformFeatureControl(
    PDEVICE_CONTEXT deviceCtx
)
{
    if (!deviceCtx)
    {
        return;
    }

    if (deviceCtx->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_LNL)
    {
        deviceCtx->PlatformFeatureControl.UseSynopsysCsiRx = 1;
        deviceCtx->PlatformFeatureControl.UseAggregatedPhy = 0;
    }
    if (deviceCtx->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_PTL)
    {
        deviceCtx->PlatformFeatureControl.UseSynopsysCsiRx = 1;
        deviceCtx->PlatformFeatureControl.UseAggregatedPhy = (deviceCtx->SensorDriverView.LanesUsed > 2) ? 1 : 0;
    }
    else if (deviceCtx->SensorDriverView.SensorDriverSubView.Platform > PLATFORM_PTL)
    {
        deviceCtx->PlatformFeatureControl.UseSynopsysCsiRx = 1;
        deviceCtx->PlatformFeatureControl.UseAggregatedPhy = (deviceCtx->SensorDriverView.LanesUsed > 2) ? 1 : 0;
    }
    else
    {
        deviceCtx->PlatformFeatureControl.UseSynopsysCsiRx = 0;
        deviceCtx->PlatformFeatureControl.UseAggregatedPhy = 0;
    }

    deviceCtx->PlatformFeatureControl.UseSensorMappingTable
        = deviceCtx->SensorDriverView.SensorDriverSubView.Platform < PLATFORM_TTL;
}

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)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s InitCameraDriverRegistryFlags pdev is NULL", DEVICE_NAME);
        return;
    }

    RtlInitUnicodeString(&CameraDriverRegistryPath, CAMERA_DRIVER_REGISTRY_PATH);
    status = OS_ReadRegistry_V2(
        &CameraDriverRegistryPath,
        (PUCHAR) "EnablePortSip2Sip3",
        REG_DWORD,
        (PUCHAR)&ulValue,
        &ulBufLen,
        POOL_TAG,
        pDevExt);
    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.", DEVICE_NAME);
    }
}

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

    if (!pDevExt || !pDevExt->DeviceHandle)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s SsInitRegistry pdev is NULL", DEVICE_NAME);
        return;
    }

    pDevExt->Key_IsRegCheck = FALSE;
    pDevExt->Key_MipiLanes = 0;
    pDevExt->Key_EnableEmbeddedData = REGISTRY_INVALID;

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

    status = OS_ReadRegistry_V2(
        &g_RegistryPath,
        (PUCHAR) "MipiLanes",
        REG_DWORD,
        (PUCHAR)&ulValue,
        &ulBufLen,
        POOL_TAG,
        pDevExt);
    if (NT_SUCCESS(status))
        pDevExt->Key_MipiLanes = (UINT8)ulValue;

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

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

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

#endif

    ulValue = 0xff;
    status = OS_ReadRegistry_V2(
        &g_RegistryPath,
        (PUCHAR) "EnableEmbeddedData",
        REG_DWORD,
        (PUCHAR)&ulValue,
        &ulBufLen,
        POOL_TAG,
        pDevExt);
    if (NT_SUCCESS(status) && (ulValue != 0xff))
        pDevExt->Key_EnableEmbeddedData = ulValue ? TRUE : FALSE;

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

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

    ulValue = 0;
    status = OS_ReadRegistry_V2(
        &g_RegistryPath,
        (PUCHAR) "DefaultPrivacyIndicatorStatus",
        REG_DWORD,
        (PUCHAR)&ulValue,
        &ulBufLen,
        POOL_TAG,
        pDevExt);
    if (NT_SUCCESS(status))
        pDevExt->SensorConf.Reserved[1] = ulValue ? TRUE : FALSE;

    ulValue = 0;
    status = OS_ReadRegistry_V2(
        &g_RegistryPath,
        (PUCHAR) "M1BootDelay",
        REG_DWORD,
        (PUCHAR)&ulValue,
        &ulBufLen,
        POOL_TAG,
        pDevExt);
    if (NT_SUCCESS(status))
    {
        pDevExt->SensorConf.Reserved[3] = ulValue;
        DoTraceMessage(FLAG_LOG, "%s M1BootDelay is set to: %d ms", DEVICE_NAME, ulValue);
    }
    else
    {
        status = OS_ReadRegistry_V2(&g_RegistryPath,
                                    (PUCHAR) "BootDelay",
                                    REG_DWORD,
                                    (PUCHAR)&ulValue,
                                    &ulBufLen,
                                    POOL_TAG,
                                    pDevExt);
        if (NT_SUCCESS(status))
        {
            pDevExt->SensorConf.Reserved[3] = ulValue;
            DoTraceMessage(FLAG_LOG, "%s BootDelay is set to: %d ms", DEVICE_NAME, ulValue);
        }
        else
        {
            /*Initilize and configured later when CV-Chip I2C are configured by BIOS*/
            pDevExt->SensorConf.Reserved[3] = 0;
        }
    }
    InitCameraDriverRegistryFlags(pDevExt);
}

void SsInitContextCommon(
    PDEVICE_CONTEXT deviceCtx
    )
{
    if (!deviceCtx)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s SsInitContextCommon pdev is NULL", DEVICE_NAME);
        return;
    }
    if (!(deviceCtx->DeviceHandle))
    {
        return;
    }

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

    deviceCtx->GuidIndex = 0;
    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 = (INT32)Sensor_Res[PreviewMode][0].MipiBps;
    deviceCtx->SensorMipiConfig.MipiLane = DefaultSensorMipiConfig.MipiLane;
    deviceCtx->SensorInterface.GetPDAFCaps = NULL;
    deviceCtx->SensorInterface.SetSensorCalibrationData = NULL;
    deviceCtx->SensorInterface.GetPDAFResolutions = NULL;
    deviceCtx->SensorInterface.SetMultipleExposure = NULL;
    deviceCtx->CVFPowerOn = FALSE;
    deviceCtx->CVFInterface = NULL;
    deviceCtx->CVFDeviceObject = NULL;

    deviceCtx->CameraSetPrivacyMode = NULL;
    deviceCtx->CameraContext = NULL;
    deviceCtx->WMIPrivacyModeNotificationObject = NULL;
    deviceCtx->WMIDeviceStateQueryObject = NULL;
    deviceCtx->QueryPrivacyState = NULL;

    deviceCtx->CameraSetFlipMode = NULL;
    deviceCtx->QueryCurrentAngle = NULL;
    deviceCtx->WMIFlipModeNotificationObject = NULL;
    deviceCtx->WMICurAngleQueryObject = NULL;
    deviceCtx->CameraHdmiStatusUpdate = NULL;
    deviceCtx->bIsHdmiConnected = FALSE;
    deviceCtx->AudioSamplingRateForMipiBridge = 0;
    deviceCtx->I2CVisionChipIndex = 0;
    deviceCtx->AdvancedSnrMode = NORMAL;
    deviceCtx->SupportModeSwitch = FALSE;
    deviceCtx->JVSNormalCombine = FALSE;
    deviceCtx->SecondSourceLensSupport = FALSE;
    deviceCtx->PowerOnOffStatus = STATUS_SUCCESS;

    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, "[ERROR] %s Get DSDT settings failed!", DEVICE_NAME);
        return;
    }
    ParseDsdtSettings(deviceCtx);

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

    SsInitExportResTable(deviceCtx);

    const char* SensorName = DEVICE_NAME;
    //For MS project
    if ((_stricmp(SensorName, "OV13858") == 0) &&
        ((_stricmp((CHAR *)&deviceCtx->DsdtSettings.ModuleName, "MSHW0261") == 0) ||
        (_stricmp((CHAR *)&deviceCtx->DsdtSettings.ModuleName, "MSHW0341") == 0) ||
        (_stricmp((CHAR *)&deviceCtx->DsdtSettings.ModuleName, "MSHW0741") == 0) ||
        (_stricmp((CHAR *)&deviceCtx->DsdtSettings.ModuleName, "MSHW0521") == 0)))
    {
        deviceCtx->SsVcm.VCMFarPosition = 650;
        deviceCtx->SsVcm.VCMHomePostion = 530;
    }
    if (_stricmp((CHAR *)&deviceCtx->DsdtSettings.ModuleName, "MSHW0522") == 0)
    {
        // 22018704875:
        // For Fraser River special HW, VD55G0 has one HW issue, give one special config in here.
        deviceCtx->SensorConf.Reserved[2] = TRUE;
    }

    InitContextVcm(&deviceCtx->SsVcm);
    InitContextNvram(&deviceCtx->SsNvm);
    InitInterfaceVcmNvm(deviceCtx);
    InitPlatformFeatureControl(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)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)

    // power off current camera module
    status = SensorPower(pDevExt, FALSE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s SensorPower Off Fail!", DEVICE_NAME);
        goto done;
    }
    SleepMSecond(20);
    // power on current camera module
    status = SensorPower(pDevExt, TRUE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s SensorPower On Fail!", DEVICE_NAME);
        goto done;
    }
done:
    pDevExt->PowerOnOffStatus = status;
    return status;
}

NTSTATUS SsSensorAcquireGuidIndex(
    PDEVICE_CONTEXT pDevExt
    )
{
    if ((pDevExt == NULL) || (pDevExt->Pld.Panel != PANEL_FRONT))
    {
        return STATUS_INVALID_PARAMETER;
    }

    for (INT32 i = 0; i < g_MaxInstanceNum; i++)
    {
        if ((g_GuidMask & (1 << i)) == 0)
        {
            g_GuidMask |= (1 << i);
            pDevExt->GuidIndex = i;
            return STATUS_SUCCESS;
        }
    }

    return STATUS_INVALID_PARAMETER;
}

NTSTATUS SsSensorReleaseGuidIndex(
    PDEVICE_CONTEXT pDevExt
    )
{
    if (pDevExt == NULL)
    {
        return STATUS_INVALID_PARAMETER;
    }

    if ((g_GuidMask & (1 << pDevExt->GuidIndex)) == 0)
    {
        return STATUS_INVALID_PARAMETER;
    }

    g_GuidMask &= ~(1 << pDevExt->GuidIndex);
    return STATUS_SUCCESS;
}

#define MAX_RELOAD_COUNT            5

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

    if (!pDevExt)
    {
        return STATUS_INVALID_PARAMETER;
    }
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    DoTraceMessage(FLAG_LOG, "%s Startup (enters) ", DEVICE_NAME);

     /*
     * ECR:448982.
     * Dell projects on LNL and PTL design on CVS, have 2 sensors connected with CV chip,
     * but only 1 sensor is for vision using.
     * This result in these 2 sensors need to call DDI before access I2C resource.
     * And the non-vision sensor need to call controllogic as well.
     */
    /*
    * This is a old WA for CVF platforms, which don't have sensor depency on CVF driver.
    * So, avoid accessing I2C before DDI call. The same as the case in ECR 448982.
    */
    if (pDevExt->CVFInterface)
    {
        DoTraceMessage(FLAG_LOG, "[CVF] %s Startup: SensorPower status = %d", DEVICE_NAME, pDevExt->CVFPowerOn);
        if (!pDevExt->CVFPowerOn)
            goto done;
    }

    // power on current camera module
    status = SensorPower(pDevExt, TRUE);
    pDevExt->PowerOnOffStatus = status;
    if (status == STATUS_PNP_DEVICE_CONFIGURATION_PENDING)
    {
        if (pDevExt->CVFPowerOn == FALSE)
        {
            DoTraceMessage(FLAG_ERROR, "[Warning] %s No valid GPIO set 0x%x!", DEVICE_NAME, status);
            status = STATUS_SUCCESS;
            pDevExt->PowerOnOffStatus = STATUS_SUCCESS;
            goto done;
        }
    }
    else if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s SensorPower On Fail!", 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, "[ERROR] %s Module Check Fail", DEVICE_NAME);
                goto done;
            }
        }

        pDevExt->DriverStage = DRIVER_STAGE_WORKING;
        DoTraceMessage(FLAG_LOG, "%s Module Check Successfully", 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, "[ERROR] %s Failed to init, reload %d", DEVICE_NAME, reload);
        }
        // init cmos sensor
        status = InitCmos(pDevExt);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s InitCmos fail", 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, "[ERROR] %s InitVcm fail", DEVICE_NAME);
            continue;
        }
        // init otp/eeprom..
        status = InitRom(&pDevExt->SsNvm);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s InitRom fail", 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);
                }
            }
        }

        // If there is module name saved in sensor EEPROM, it will be parsed in InitRom, and the module name is saved in
        // pDevExt->SsNvm.ModuleInfo. LoadModuleInfo function will check if we need to use module name in EEPROM.
        LoadModuleInfo(pDevExt);

        status = GetModuleInfoFromOTP(pDevExt);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s Module Info from OTP is invalid", 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(pDevExt);
    DoTraceMessage(FLAG_LOG, "%s Startup (exit) 0x%x", DEVICE_NAME, status);
    return status;
}

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

    CHECK_POINTER(pDevExt)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
#ifdef PIXTER_DEFINE
    UnInitCmos(pDevExt);
#endif
    if (pDevExt->CVFInterface)
    {
        DoTraceMessage(FLAG_LOG, "[CVF] %s Shutdown: SensorPower status = %d", DEVICE_NAME, pDevExt->CVFPowerOn);
        if (pDevExt->CVFPowerOn)
            goto done;
    }
    // power off current camera module
    status = SensorPower(pDevExt, FALSE);
    pDevExt->PowerOnOffStatus = status;
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s SensorPower Off Fail!", DEVICE_NAME);
    }

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

NTSTATUS Cmd_Idle(
    PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS  status = STATUS_SUCCESS;
    CHECK_POINTER(pDevExt)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    DoTraceMessage(FLAG_LOG, "%s Cmd_Idle+", DEVICE_NAME);

    status = ReleaseVisionSensor(pDevExt);
    if (!NT_SUCCESS(status))
    {
        goto done;
    }
    /*
    * 1) Sensor D0Entry failed can cause removal of sensor to make the DeviceHandle to be NULL.
    *    Need to check the validation of DeviceHandle here, in case sensor was removed before calling this.
    * 2) Sensor power failure when driver verifier is enabled can also cause the IRQL to DIRQL, which is higher than DISPATCH_LEVEL.
    *    WdfDeviceResumeIdle can't run in IRQL > DISPATCH_LEVEL. https://learn.microsoft.com/en-us/windows-hardware/drivers/kernel/managing-hardware-priorities
    */
    KIRQL irql = KeGetCurrentIrql();
    if (irql <= DISPATCH_LEVEL && pDevExt->DeviceHandle)
    {
        WdfDeviceResumeIdle(pDevExt->DeviceHandle);
    }

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

        pDevExt->ControlLogicInterface->InterfaceHeader.InterfaceDereference(
            pDevExt->ControlLogicInterface->InterfaceHeader.Context);
    }

done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_Idle-", DEVICE_NAME);
    return status;
}

NTSTATUS OnCVFPrivacyNotification(PVOID Context, UINT32 PrivacyState)
{
    PDEVICE_CONTEXT pDevCtx = Context;

    TraceWPPLogFn(FLAG_LOG, "CVF Driver Notification, PrivacyState: %d.", PrivacyState);

    if (pDevCtx->CameraSetPrivacyMode)
        return pDevCtx->CameraSetPrivacyMode(pDevCtx->CameraContext, PrivacyState);

    return STATUS_SUCCESS;
}

NTSTATUS Cmd_Active(
    PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS  status = STATUS_SUCCESS;
    CHECK_POINTER(pDevExt)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)

    DoTraceMessage(FLAG_LOG, "%s Cmd_Active+", DEVICE_NAME);

    status = AcquireVisionSensor(pDevExt);
    if (!NT_SUCCESS(status))
    {
        goto done;
    }

    if (pDevExt->QueryPrivacyState)
    {
        ULONG privacyState = 0;
        status = pDevExt->QueryPrivacyState(pDevExt, &privacyState);
        if (NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_LOG, "%!FUNC!, %s, privacy state=0x%x", SENSOR_NAME, privacyState);
            if (pDevExt->CameraSetPrivacyMode)
            {
                pDevExt->CameraSetPrivacyMode(pDevExt->CameraContext, !!privacyState);
            }
            else
            {
                DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC!, %s, doesn't have CameraSetPrivacyMode callback", SENSOR_NAME);
            }
            // Do not set resolution when start streaming with privacy ON
            // Need to set resolution during recovery process of exiting privacy flow
            pDevExt->bRecoverResolution = privacyState;
        }
        else
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC!, %s, query privacy state failed, status=0x%x, so turn to normal flow", SENSOR_NAME, status);
        }
    }

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

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

done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_Active-", DEVICE_NAME);
    return status;
}

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

    CHECK_POINTER(pDevExt)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    DoTraceMessage(FLAG_LOG, "%s Cmd_Startup+ x", DEVICE_NAME);
    if (pDevExt->SensorConf.Reserved[1] && pDevExt->ControlLogicInterface)
    {
        if (pDevExt->SKU.crd == SKU_CRD_G || pDevExt->SKU.crd == SKU_CRD_G2 || pDevExt->SKU.crd == SKU_CRD_D)
            pDevExt->ControlLogicInterface->PrivacyLedOff(
                pDevExt->ControlLogicInterface->InterfaceHeader.Context, &pDevExt->SensorConf);
    }

    status = Startup(pDevExt);

done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_Startup- returns with code: 0x%x", DEVICE_NAME, status);
    return status;
}

NTSTATUS Cmd_Shutdown(PDEVICE_CONTEXT pDevExt)
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(pDevExt)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    DoTraceMessage(FLAG_LOG, "%s Cmd_Shutdown+ x", DEVICE_NAME);

    // Reset focus vcm
    ResetVcmPosition(&pDevExt->SsVcm);
    ReleaseRom(&pDevExt->SsNvm);
    status = Shutdown(pDevExt);
    if (!NT_SUCCESS(status))
        goto done;

done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_Shutdown- returns with code: 0x%x", 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 +", DEVICE_NAME);
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)

    if (*Size > sizeof(SENSOR_MODE_DATA))
    {
        *Size = sizeof(SENSOR_MODE_DATA);
    }

    status = GetTimingFactor(pDevExt, (PSENSOR_MODE_DATA)Buffer);

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


// for requirements of AI cards through I2C control BEFORE configuring sensor setting
NTSTATUS PreConfigCVChip(PDEVICE_CONTEXT pDevExt)
{
    NTSTATUS status = STATUS_UNSUCCESSFUL;

    if (!pDevExt)
    {
        DoTraceMessage(FLAG_ERROR, "%s: Invalid device context!", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }

    switch (pDevExt->I2CVisionChipIndex)
    {
        case I2C_ALTEK:
        status = CommonRegWrite(pDevExt, REG_M1_PNP_OP, M1_PNP_STOP, DATA_8BITS, pDevExt->I2CVisionChipIndex);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s M1 PNP stop failed! Status: 0x%08X", DEVICE_NAME, status);
                break;
            }

            status = CommonRegWrite(pDevExt, REG_M1_SENSOR_MODE, pDevExt->AdvancedSnrMode, DATA_8BITS, pDevExt->I2CVisionChipIndex);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s M1 mode set failed! Mode: %d, Status: 0x%08X", DEVICE_NAME, pDevExt->AdvancedSnrMode, status);
            }
            break;

        case I2C_HIMAX:
        {
            int       sensorMode = WE2_MODE_NORMAL;
            const int extendOpt = Sensor_Res[pDevExt->Mode][pDevExt->ResIndex].ExtendOpt[2];

            if (extendOpt == NORMAL)
            {
                sensorMode = WE2_MODE_NORMAL;
            }
            else if (extendOpt == STAGGER_HDR)
            {
                sensorMode = WE2_MODE_HDR;
            }

            status = CommonRegWrite(pDevExt, REG_WE2_SENSOR_MODE, sensorMode, DATA_8BITS, pDevExt->I2CVisionChipIndex);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "%s W2 sensor mode notification Fail with code: 0x%x! ", DEVICE_NAME, status);
            }
            // T2 delay to wait for WE2 ready
            SleepMSecond(WE2_HANDSHAKE_DELAY_T2);
            break;
        }

        default:
        {
            status = STATUS_SUCCESS;
            break;
        }
    }

    if (NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s Pre-config CV-Chip success for I2CAICIndex: %d", DEVICE_NAME, pDevExt->I2CVisionChipIndex);
    }

    return status;
}

NTSTATUS PostConfigCVChip(PDEVICE_CONTEXT pDevExt)
{
    NTSTATUS status = STATUS_UNSUCCESSFUL;

    if (!pDevExt)
    {
        DoTraceMessage(FLAG_ERROR, "%s: Invalid device context!", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }

    switch (pDevExt->I2CVisionChipIndex)
    {
    case I2C_ALTEK:
    {
        status = CommonRegWrite(pDevExt, REG_M1_PNP_OP, M1_PNP_START, DATA_8BITS, pDevExt->I2CVisionChipIndex);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%s M1 PNP Start Fail with code: 0x%x", DEVICE_NAME, status);
        }
        break;
    }
    case I2C_HIMAX:
    {
        //no post config for HIMAX WE2
        status = STATUS_SUCCESS;
        break;
    }
    default:
        status = STATUS_SUCCESS;
        break;
    }

    if (NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s Post-config CV-Chip success for I2CAICIndex: %d", DEVICE_NAME, pDevExt->I2CVisionChipIndex);
    }
    return status;
}

// for requirements of AI cards through I2C control AFTER configuring sensor setting
NTSTATUS PostConfigSensorSetting(PDEVICE_CONTEXT pDevExt, ULONG Width, ULONG Height, LONG FrameRate)
{
    NTSTATUS status = STATUS_SUCCESS;
    // #1 AI Chip Configuration
    status = PostConfigCVChip(pDevExt);

    // #2 new DDI for SetMipiConfig
    // send MIPI config to Vision driver, but ignore the result value, as some CV chip may not support this, but still can stream.
    if (pDevExt->SensorCaps.CVFSupport && pDevExt->CVFInterface &&
        ((PINTERFACE)(pDevExt->CVFInterface))->Version > VISION_INTERFACE_VERSION_12)
    {
        if (pDevExt->CVFPowerOn)
        {
            MIPI_DATA_CONFIG mipiDataConfig = {0};
            if (Sensor_Res[pDevExt->Mode][pDevExt->ResIndex].ExtendOpt[2] == STAGGER_HDR)
            {
                mipiDataConfig.fps = (UINT8)(FrameRate * 2);
            }
            else
            {
                mipiDataConfig.fps = (UINT8)FrameRate;
            }

            mipiDataConfig.laneCount = (UINT8)pDevExt->SensorMipiConfig.MipiLane;
            mipiDataConfig.phyMode = (UINT8)pDevExt->SensorMipiExt.PhyConfig;
            mipiDataConfig.mipiClk = pDevExt->MipiBps;
            mipiDataConfig.vcCount = pDevExt->AdvancedSnrMode == STAGGER_HDR ? 2 : 1; // todo: add VC count support
            DoTraceMessage(FLAG_LOG,
                           "%!FUNC! laneCount = %d, phyMode = %d, mipiClk = %d, vcCount = %d",
                           mipiDataConfig.laneCount,
                           mipiDataConfig.phyMode,
                           mipiDataConfig.mipiClk,
                           mipiDataConfig.vcCount);
            unsigned char dtIndex = 0;
            for (unsigned int vcIndex = 0; vcIndex < mipiDataConfig.vcCount; vcIndex++)
            {
                // raw
                /*
                 * For DOL case, it will be VC0 VC1 for long and short exposure image.
                 * For MIPI aggregation case, IR sensor should always be in VC2, and VC0 and VC1 are for DOL, or VC 0
                 * for normal bayer.
                 */
                if (mipiDataConfig.vcCount == 1)
                {
                    mipiDataConfig.vcConfigs[vcIndex].vcChannel = (UINT8)pDevExt->VCID;
                }
                else
                {
                    mipiDataConfig.vcConfigs[vcIndex].vcChannel = (UINT8)vcIndex;
                }
                dtIndex = mipiDataConfig.vcConfigs[vcIndex].dtCount;
                mipiDataConfig.vcConfigs[vcIndex].dtCount += 1;
                mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelWidth = (UINT16)Width;
                mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelHeight = (UINT16)Height;
                if (pDevExt->SensorMipiExt.OutputFormat == INPUT_FORMAT_RAW_10)
                {
                    mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].dataType = IPU_ISYS_MIPI_DATA_TYPE_RAW_10;
                }
                else if (pDevExt->SensorMipiExt.OutputFormat == INPUT_FORMAT_RAW_8)
                {
                    mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].dataType = IPU_ISYS_MIPI_DATA_TYPE_RAW_8;
                }
                else
                {
                    status = STATUS_INVALID_PARAMETER;
                    DoTraceMessage(FLAG_ERROR, "%!FUNC! invalid OutputFormat %d, just return!", pDevExt->SensorMipiExt.OutputFormat);
                    goto done;
                }
                DoTraceMessage(FLAG_LOG,
                               "%!FUNC! raw vcIndex = %d, vcChannel = %d, dtcount = %d, dataType = %d",
                               vcIndex,
                               mipiDataConfig.vcConfigs[vcIndex].vcChannel,
                               dtIndex + 1,
                               mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].dataType);
                // metadata
                if (pDevExt->SensorCaps.EmbeddedLineNum > 0)
                {
                    dtIndex = mipiDataConfig.vcConfigs[vcIndex].dtCount;
                    mipiDataConfig.vcConfigs[vcIndex].dtCount += 1;

                    EMB_DATA_INFO embDataInfo = {0};
                    if (NT_SUCCESS(pDevExt->SensorInterface.GetExtFeature(pDevExt,
                                                                          SNSR_EXT_FEATURE_EMBDATA_INFO,
                                                                          (PULONG)(&embDataInfo))))
                    {
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].dataType = (UINT8)embDataInfo.DataFormat;
                    }
                    else
                    {
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].dataType = IPU_ISYS_MIPI_DATA_TYPE_RAW_10;
                    }

                    if (mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].dataType == IPU_ISYS_MIPI_DATA_TYPE_RAW_10)
                    {
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelWidth = (UINT16)(Width * 10 / 8);
                    }
                    else // RAW 8
                    {
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelWidth = (UINT16)Width;
                    }

                    mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelHeight =
                        (UINT16)pDevExt->SensorCaps.EmbeddedLineNum;

                    DoTraceMessage(
                        FLAG_LOG,
                        "%!FUNC! metadata dtcount = %d, dataType = %d resolutionWidth = %d resolutionHeight = %d",
                        dtIndex + 1,
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].dataType,
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelWidth,
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelHeight);
                }

                // PDAF T2
                if (pDevExt->SensorCaps.PDAFType == PDAF_SENSOR_T2)
                {
                    dtIndex = mipiDataConfig.vcConfigs[vcIndex].dtCount;
                    mipiDataConfig.vcConfigs[vcIndex].dtCount += 1;

                    PDAF_CAPS *pdafCap = NULL;
                    if (NT_SUCCESS(pDevExt->SensorInterface.GetPDAFCaps(pDevExt, &pdafCap)))
                    {
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].dataType = pdafCap->data_type;
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelWidth =
                            (UINT16)pdafCap->pdaf_resolution.sensorWidth;
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelHeight =
                            (UINT16)pdafCap->pdaf_resolution.sensorHeight;
                    }
                    DoTraceMessage(
                        FLAG_LOG,
                        "%!FUNC! metadata dtcount = %d, dataType = %d resolutionWidth = %d resolutionHeight = %d",
                        dtIndex + 1,
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].dataType,
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelWidth,
                        mipiDataConfig.vcConfigs[vcIndex].dtConfigs[dtIndex].pixelHeight);
                }
            }
            NTSTATUS tStatus = ((PXSENSOR_DRV_INTERFACE)(pDevExt->CVFInterface))->control(((PINTERFACE)(pDevExt->CVFInterface))->Context, XSD_MIPI_CFG, &mipiDataConfig);
            DoTraceMessage(FLAG_LOG, "%s CVF XSD_MIPI_CFG - status: 0x%x", DEVICE_NAME, tStatus);
        }
        else
        {
            DoTraceMessage(FLAG_LOG, "%!FUNC! won't call CVF XSD_MIPI_CFG as CVF is not powered on!");
        }
    }

done:
    return status;
}

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

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

    if (Mode == PreviewModeSwitch)
    {
        Mode = PreviewMode;
        Remap = TRUE;
    }

    if (Mode == StillModeSwitch)
    {
        Mode = StillMode;
        Remap = TRUE;
    }

    if (Mode == VideoModeSwitch)
    {
        Mode = VideoMode;
        Remap = TRUE;
    }
    if (Mode == RawModeModeSwitch)
    {
        Mode = RawMode;
        Remap = TRUE;
    }
    if (pDevExt->PowerOnStatus != PWR_UP)
    {
        status = Startup(pDevExt);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_LOG, "%s pDevExt startup Fail! ", DEVICE_NAME);
            return status;
        }
    }

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

    status = CVFPowerCheck(pDevExt);
    CHECK_STATUS(status);

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

    status = PreConfigCVChip(pDevExt);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s pre config CV Chip failure! ", DEVICE_NAME);
        goto done;
    }

    //Re-map sensor setting idx with mode switching supported
    if (pDevExt->AdvancedSnrMode != NORMAL)
    {
        ResIndexRemapping(pDevExt, Mode, Width, Height, frameRate, &idx);
    }
    if (!Remap)
    {
        status = InitResolution(pDevExt, Mode, idx);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_LOG, "%s SetResolution Fail! ", 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;

    status = PostConfigSensorSetting(pDevExt, Width, Height, frameRate);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s post config sensor setting failure! ", DEVICE_NAME);
        goto done;
    }

done:
    DoTraceMessage(FLAG_LOG, "%s SetResolution index = %d ", DEVICE_NAME, pDevExt->ResIndex);
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_SetResolution- (%d x %d, ) returns with code: 0x%x", DEVICE_NAME,
        Width, Height, status);

    return status;
}

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

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

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

    status = CVFPowerCheck(pDevExt);
    CHECK_STATUS(status);

    status = InitResolution(pDevExt, Mode, Index);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s Cmd_SetResolutionIndex Fail! ", 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 ", DEVICE_NAME, pDevExt->ResIndex);
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_SetResolutionIndex- (%d) returns with code: 0x%x", DEVICE_NAME,
        Index, status);
    return status;
}


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_GetExtentionPath(
    PDEVICE_CONTEXT pDevExt,
    PSNSR_INFO pInfo
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    WCHAR tmpPath[MAX_PATH];
    ULONG ulBufLen = sizeof(pInfo->extensionPath);
    status = OS_ReadRegistry_V2(&g_RegistryPath, (PUCHAR)"extensionPath", REG_SZ, (PUCHAR)pInfo->extensionPath, &ulBufLen, POOL_TAG, pDevExt);
    if (NT_SUCCESS(status))
    {
        //"extensionPath" should be "<drive letter>:\<path>\<to>
        RtlStringCbCatNW(pInfo->extensionPath, sizeof(pInfo->extensionPath), L"\\", sizeof(WCHAR));
        //Kernel mode driver need to add "\??\" to read.
        RtlStringCbPrintfW(tmpPath, sizeof(tmpPath), L"\\??\\");
        RtlStringCbCatNW(tmpPath, sizeof(tmpPath), pInfo->extensionPath, sizeof(pInfo->extensionPath));
        RtlStringCbCopyNW(pInfo->extensionPath, sizeof(pInfo->extensionPath), tmpPath, sizeof(tmpPath));
    }
    return status;
}


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

    CHECK_POINTER(pDevExt)
    CHECK_POINTER(pInfo)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetSensorInfo+", DEVICE_NAME);
    pInfo->guid = pDevExt->Guid;
    pInfo->type = (char)pDevExt->SensorMipiConfig.Type;
    pInfo->format = (char)pDevExt->SensorMipiConfig.OutputFormat;
    pInfo->flashID = pDevExt->SensorDriverView.SensorDriverSubView.FlashId;
    pInfo->PPRValue = pDevExt->SensorDriverView.SensorDriverSubView.PPRValue;

    RtlStringCbCopyA(pInfo->cmos, sizeof(pInfo->cmos), DEVICE_NAME);
    RtlStringCbCopyA(pInfo->module, sizeof(pInfo->module), (CHAR*)pDevExt->DsdtSettings.ModuleName);
    if (pDevExt->SsNvm.IsModuleInfoAvailable)
    {
        pInfo->moduleDetectInfo.state = MODULE_NAME_EEPROM_SUPPORTED;
        RtlStringCbCopyA(pInfo->moduleDetectInfo.eepromModuleName, sizeof(pInfo->moduleDetectInfo.eepromModuleName),
            (CHAR *)pDevExt->SsNvm.ModuleInfo.ModuleName);
        RtlStringCbCopyA(pInfo->moduleDetectInfo.dsdtModuleName, sizeof(pInfo->moduleDetectInfo.dsdtModuleName),
            (CHAR *)pDevExt->DsdtSettings.ModuleName);
        DoTraceMessage(FLAG_LOG, "Cmd_GetSensorInfo: Module detect info [state %d, dsdt name %s, eeprom name %s, module name in-use %s]",
            pInfo->moduleDetectInfo.state, pInfo->moduleDetectInfo.dsdtModuleName, pInfo->moduleDetectInfo.eepromModuleName, pInfo->module);
    }
    else
    {
        pInfo->moduleDetectInfo.state = MODULE_NAME_EEPROM_NOT_SUPPORTED;
    }
    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_6EP == pDevExt->PlatformInfo.ipuVer) || (IPU_6 == pDevExt->PlatformInfo.ipuVer))
            {
                RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "TGL");
                pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
            }
        }
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_TGL)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "TGL");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_TGL_H)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "TGL_H");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_JSLP)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "JSLP");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_ADL)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "ADL");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_MTL)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "MTL");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_LNL)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "LNL");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_PTL)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "PTL");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_NVL)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "NVL");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }
    else if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_TTL)
    {
        RtlStringCbCopyA(pInfo->info, sizeof(pInfo->info), "TTL");
        pInfo->controllogic_id = pDevExt->SensorDriverView.SensorDriverSubView.ControlLogicID;
    }

    //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");
    }
    status = Cmd_GetExtentionPath(pDevExt, pInfo);
    if (status != STATUS_SUCCESS)
    {
        DoTraceMessage(FLAG_LOG, "%s Cmd_GetExtentionPath, query ExtensionPath fail, status = 0x%x", DEVICE_NAME, status);
    }

    RtlStringCbCopyW(pInfo->driverpath, sizeof(pInfo->driverpath), pDevExt->DriverPath.Buffer);

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetSensorInfo- (%s)(%s) returns with code: 0x%x", 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;
    CHECK_POINTER_INVALID(pDevExt);

    CHECK_POINTER_INVALID(Port)
    CHECK_POINTER_INVALID(LaneNum)
    CHECK_POINTER_INVALID(OutputFormat)
    if (!pDevExt->DeviceHandle)
        return;

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

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetMIPIConfigs, port=%d, lanes=%d, format=%d ", 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)",
        DEVICE_NAME, Port, LaneNum, OutputFormat);
}

void Cmd_GetMIPIConfigsExtend(
    PDEVICE_CONTEXT pDevExt,
    SNSR_MIPI_EXT_DATA * MipiExtend
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    UNREFERENCED_PARAMETER(pDevExt);

    CHECK_POINTER_INVALID(pDevExt)
    CHECK_POINTER_INVALID(MipiExtend)
    memset(MipiExtend, 0x0, sizeof(SNSR_MIPI_EXT_DATA));
    MipiExtend->LaneConfiguration = pDevExt->SensorMipiExt.LaneConfiguration;
    MipiExtend->LaneNum = pDevExt->SensorMipiExt.LaneNum;
    MipiExtend->OutputFormat = pDevExt->SensorMipiExt.OutputFormat;
    MipiExtend->PhyConfig = pDevExt->SensorMipiExt.PhyConfig;
    MipiExtend->Port = pDevExt->SensorMipiExt.Port;
    MipiExtend->Type = pDevExt->SensorMipiExt.Type;
    MipiExtend->StopStateType = pDevExt->SensorMipiExt.StopStateType; //IMX688 enter to LP000(not stop state) before start view finder
        DoTraceMessage(FLAG_LOG, "%s Cmd_GetMIPIConfigsExtend, LaneConfiguration=0d%d, LaneNum=0d%d, OutputFormat=0d%d PhyConfig=0d%d Port=0d%d Type=%d StopStateType=%d", DEVICE_NAME,
            pDevExt->SensorMipiExt.LaneConfiguration, pDevExt->SensorMipiExt.LaneNum,
            pDevExt->SensorMipiExt.OutputFormat, pDevExt->SensorMipiExt.PhyConfig,
            pDevExt->SensorMipiExt.Port, pDevExt->SensorMipiExt.Type,
            pDevExt->SensorMipiExt.StopStateType);

    return;
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetMIPIConfigsExtend invalid parameter pDevExt(0x%p), MipiExtend(0x%p)",
        DEVICE_NAME, pDevExt, MipiExtend);
    return;
}

NTSTATUS Cmd_GetCaps (
    PDEVICE_CONTEXT pDevExt,
    SNSR_CAPS **Caps
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(pDevExt)
    CHECK_POINTER(Caps)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    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", 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)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    *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", 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)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    status = Detect(pDevExt, Version);
    if (!NT_SUCCESS(status))
        goto done;

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetVersion Sensor Id = 0x%x", 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;
    }
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)

#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", 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 +", 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, "[ERROR] IoGetDevicePropertyData returned 0x%x", 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", 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)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    //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, ", DEVICE_NAME, aestore);
done:
    return status;
}

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

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

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetBinningMode binning, buffer pointer = 0x%x,  returns with code: 0x%x",
        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)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    DoTraceMessage(FLAG_LOG, "%s Cmd_StartViewfinder+", DEVICE_NAME);

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

    status = CVFPowerCheck(pDevExt);
    CHECK_STATUS(status);

    status = Stream(pDevExt, TRUE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s start viewfinder error! ", DEVICE_NAME);
        goto done;
    }
    pDevExt->Streaming = TRUE;
    pDevExt->FirstExposure = FALSE;

done:
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_StartViewfinder- (%d x %d, ) returns with code: 0x%x", DEVICE_NAME,
        Width, Height, status);
    return status;
}

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

    CHECK_POINTER(SensorCtx)
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)
    status = RegWrite(SensorCtx, Addr, Data, DATA_8BITS);

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

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

    CHECK_POINTER(SensorCtx)
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)
#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", 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)
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)
    status = RegRead(SensorCtx, Addr, &data, DATA_8BITS);
done:
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s Cmd_ReadRegWord failed 0x%x", DEVICE_NAME, status);
    }
    else
    {
        *pData = (UINT16)data;
        DoTraceMessage(FLAG_LOG, "%s Cmd_ReadRegWord addr 0x%x, data 0x%x", 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)
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)
#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", DEVICE_NAME, Addr, data);
    return status;
}


NTSTATUS Cmd_BulkWriteReg(
    PDEVICE_CONTEXT SensorCtx,
    PSNSR_REG_LIST pInputReg
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(SensorCtx)
    CHECK_POINTER(pInputReg)
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)
    for (unsigned int i = 0; i < pInputReg->RegNum; i++)
    {
        switch (pInputReg->Regs[i].width)
        {
        case SREG_8BIT:
            status = RegWrite(SensorCtx, pInputReg->Regs[i].address, (ULONG)(pInputReg->Regs[i].value), DATA_8BITS);
            DoTraceMessage(FLAG_LOG, "%s Cmd_BulkWriteReg SREG_8BIT addr 0x%x, data 0x%x", DEVICE_NAME, pInputReg->Regs[i].address, pInputReg->Regs[i].value);
            break;
        case SREG_16BIT:
            status = RegWrite(SensorCtx, pInputReg->Regs[i].address, (ULONG)(pInputReg->Regs[i].value), DATA_16BITS);
            DoTraceMessage(FLAG_LOG, "%s Cmd_BulkWriteReg SREG_16BIT addr 0x%x, data 0x%x", DEVICE_NAME, pInputReg->Regs[i].address, pInputReg->Regs[i].value);
            break;
        case SREG_32BIT:
            status = RegWrite(SensorCtx, pInputReg->Regs[i].address, (ULONG)(pInputReg->Regs[i].value), DATA_32BITS);
            DoTraceMessage(FLAG_LOG, "%s Cmd_BulkWriteReg SREG_32BIT addr 0x%x, data 0x%x", DEVICE_NAME, pInputReg->Regs[i].address, pInputReg->Regs[i].value);
            break;
        default:
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s %s out of width range", DEVICE_NAME, __FUNCTION__);
            break;
        }
    }

done:
    return status;
}

NTSTATUS Cmd_BulkReadReg(
    PDEVICE_CONTEXT SensorCtx,
    PSNSR_REG_LIST pInputReg,
    PSNSR_REG_LIST pOutputReg
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    SNSR_REG_LIST readRegs;

    CHECK_POINTER(SensorCtx)
    CHECK_POINTER(pInputReg)
    CHECK_POINTER(pOutputReg)
    CHECK_DEVICE_HANDLE(SensorCtx->DeviceHandle)
    readRegs.RegNum = pInputReg->RegNum;
    for (unsigned int i = 0; i < pInputReg->RegNum; i++)
    {
        readRegs.Regs[i].address = pInputReg->Regs[i].address;
        readRegs.Regs[i].width = pInputReg->Regs[i].width;
        switch (pInputReg->Regs[i].width)
        {
        case SREG_8BIT:
            status = RegRead(SensorCtx, pInputReg->Regs[i].address, &(ULONG)(readRegs.Regs[i].value), DATA_8BITS);
            DoTraceMessage(FLAG_LOG, "%s Cmd_BulkReadReg SREG_8BIT addr 0x%x, data 0x%x", DEVICE_NAME, pInputReg->Regs[i].address, readRegs.Regs[i].value);
            break;
        case SREG_16BIT:
            status = RegRead(SensorCtx, pInputReg->Regs[i].address, &(ULONG)(readRegs.Regs[i].value), DATA_16BITS);
            DoTraceMessage(FLAG_LOG, "%s Cmd_BulkReadReg SREG_16BIT addr 0x%x, data 0x%x", DEVICE_NAME, pInputReg->Regs[i].address, readRegs.Regs[i].value);
            break;
        case SREG_32BIT:
            status = RegRead(SensorCtx, pInputReg->Regs[i].address, &(ULONG)(readRegs.Regs[i].value), DATA_32BITS);
            DoTraceMessage(FLAG_LOG, "%s Cmd_BulkReadReg SREG_32BIT addr 0x%x, data 0x%x", DEVICE_NAME, pInputReg->Regs[i].address, readRegs.Regs[i].value);
            break;
        default:
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s %s out of width range", DEVICE_NAME, __FUNCTION__);
            break;
        }
    }

    if (!NT_SUCCESS(status))
        goto done;

    memcpy_s(pOutputReg, sizeof(SNSR_REG_LIST), &readRegs, sizeof(SNSR_REG_LIST));

done:
    return status;
}


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

    CHECK_POINTER_INVALID(pDevExt)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    DoTraceMessage(FLAG_LOG, "%s Cmd_StopViewfinder+", DEVICE_NAME);

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

    status = CVFPowerCheck(pDevExt);
    CHECK_STATUS(status);

    // Viewfinder off
    pDevExt->Streaming = FALSE;
    pDevExt->FirstExposure = TRUE;
    status = Stream(pDevExt, FALSE);

done:
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s stop viewfinder error! begin to shutdown!", DEVICE_NAME);
        status = Shutdown(pDevExt);
    }
invalid:
    DoTraceMessage(FLAG_LOG, "%s Cmd_StopViewfinder- returns with code: 0x%x", 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)
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    ULONG ulBufLen = 0;
    switch (feature)
    {
    case SNSR_EXT_FEATURE_MIPICLK:
        //mipi bit rate per lane
        ULONG MipiBps = 0;
        ULONG BufLen = sizeof(MipiBps);
        status = OS_ReadRegistry_V2(
            &g_RegistryPath,
            (PUCHAR) "MipiBps",
            REG_DWORD,
            (PUCHAR)&MipiBps,
            &BufLen,
            POOL_TAG,
            pDevExt);
        if (NT_SUCCESS(status) && (MipiBps >= MIPI_BPS_M_LOWEST) && (MipiBps <= MIPI_BPS_M_BIGEST))
        {
            *pValue = MipiBps * MIPI_BPS_M_UNIT;
            DoTraceMessage(FLAG_LOG, "%s get MipiBps from registry = %d", DEVICE_NAME, *pValue);
        }
        else
        {
            *pValue = pDevExt->MipiBps;
            // Reset return status, in case OS_ReadRegistry failed
            status = STATUS_SUCCESS;
            DoTraceMessage(FLAG_LOG, "%s get MipiBps from sensor = %d", DEVICE_NAME, *pValue);
        }
        DoTraceMessage(FLAG_LOG, "%s Cmd_GetExtFeature Mipi Clock = %u,  returns with code: 0x%x", DEVICE_NAME, *pValue, status);
        break;
    case SNSR_EXT_FEATURE_CVF_POWER:
        if (pDevExt->SensorCaps.CVFSupport)
        {
            *pValue = pDevExt->CVFPowerOn;
            DoTraceMessage(FLAG_LOG, "%s %!FUNC!, CVFPowerOn = %d", DEVICE_NAME, *pValue);
        }
        else
        {
            status = STATUS_NOT_SUPPORTED;
        }
        break;
    case SNSR_EXT_FEATURE_CVF_AVAILABILITY:
        if (pDevExt->SensorCaps.CVFSupport)
        {
            *pValue = pDevExt->CVFDeviceObject && pDevExt->CVFInterface;
        }
        else
        {
            *pValue = FALSE;
        }
        DoTraceMessage(FLAG_LOG, "%s %!FUNC!, CVF availability = %d", DEVICE_NAME, *pValue);
        break;
    case SNSR_EXT_FEATURE_CVF_SUPPORT:
        if (pDevExt->SensorCaps.CVFSupport)
        {
            *pValue = TRUE;
        }
        else
        {
            *pValue = FALSE;
        }
        DoTraceMessage(FLAG_LOG, "%s %!FUNC!, CVF support = %d", DEVICE_NAME, *pValue);
        break;
    case SNSR_EXT_FEATURE_PRIVACY_SHUTTER_SUPPORT:
        *pValue = FALSE;
        DoTraceMessage(FLAG_LOG, "%s %!FUNC!, Hardware privacy shutter support = %d", DEVICE_NAME, *pValue);
        break;
    case SNSR_EXT_FEATURE_CAMERA_MASK:
        ULONG bIsSupportCameraMask = 0;
        ulBufLen = sizeof(bIsSupportCameraMask);
        status = OS_ReadRegistry_V2(
            &g_RegistryPath,
            (PUCHAR) "IsSupportCameraMask",
            REG_DWORD,
            (PUCHAR)&bIsSupportCameraMask,
            &ulBufLen,
            POOL_TAG,
            pDevExt);
        if (NT_SUCCESS(status) && bIsSupportCameraMask == 1)
        {
            *pValue = 1;
        }
        else
        {
            *pValue = 0;
        }
        DoTraceMessage(FLAG_LOG, "%s Camera Mask Mode Enabled = 0x%x", DEVICE_NAME, *pValue);
        break;
    case SNSR_EXT_FEATURE_JVS_PIPE:
        *pValue = FALSE;
        DoTraceMessage(FLAG_LOG, "%s JVS enabled = 0x%x,  returns with code: 0x%x\n", DEVICE_NAME, *pValue, status);
        break;
    case SNSR_EXT_FEATURE_MODE_SWITCH:
        *pValue = pDevExt->SupportModeSwitch;
        DoTraceMessage(FLAG_LOG, "%s Mode switch supported = 0x%x, ", DEVICE_NAME, *pValue);
        break;
    case SNSR_EXT_FEATURE_MODE_NOTIFICATION:
        *pValue = pDevExt->AdvancedSnrMode;
        DoTraceMessage(FLAG_LOG, "%s Get Advanced sensor mode = 0x%x,  returns with code: 0x%x", DEVICE_NAME, *pValue, status);
        break;
    case SNSR_EXT_FEATURE_GET_SNR_MODE_BOUNDARY:
        *pValue = 1920 * 1080;
        DoTraceMessage(FLAG_LOG, "%s Default sensor mode boundary = 0x%x,  returns with code: 0x%x", DEVICE_NAME, *pValue, status);
        break;
    case SNSR_EXT_FEATURE_AESETTING_TOCVCHIP:
        ULONG bEnableCLS = 0;
        *pValue = FALSE;
        ulBufLen = sizeof(bEnableCLS);
        status = OS_ReadRegistry_V2(&g_RegistryPath,
                                    (PUCHAR) "EnableCLS",
                                    REG_DWORD,
                                    (PUCHAR)&bEnableCLS,
                                    &ulBufLen,
                                    POOL_TAG,
                                    pDevExt);

        if (NT_SUCCESS(status) && !!bEnableCLS)
        {
            const AE_SETTING dummy_ae_setting = {0.0, 0.0, 0.0};
            status = SetAEToCVChip(pDevExt, dummy_ae_setting);
            *pValue = (status != STATUS_NOT_SUPPORTED);
            DoTraceMessage(FLAG_LOG, "%s CLS support = 0x%x", DEVICE_NAME, *pValue);
        }
        else
        {
            DoTraceMessage(FLAG_LOG, "Not CLS request HW config, disable this feature");
        }
        break;
    default:
        status = STATUS_NOT_SUPPORTED;
        break;
    }
done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetExtFeature (ID = %d) returns with code: 0x%x", DEVICE_NAME, feature, status);
    return status;
}

NTSTATUS Cmd_GetFeature(PDEVICE_CONTEXT PContext, GUID Guid, PULONG PValue)
{
    if (!PContext || !PValue)
    {
        return STATUS_INVALID_PARAMETER;
    }

    NTSTATUS status = STATUS_NOT_SUPPORTED;
    if (IsEqualGUID(&Guid, &GUID_SNR_FEATURE_ACS_SUPPORT))
    {
        // Prefer to use module name to decide ACS support or not
        ULONG value = 0;
        ULONG size = sizeof(value);
        status = OS_ReadRegistry_V2(&g_RegistryPath, (PUCHAR) "EnableACSSupport", REG_DWORD, (PUCHAR)&value,
                                    &size, POOL_TAG, PContext);

        if (NT_SUCCESS(status) && value > 0)
        {
            *PValue = TRUE;
            status = STATUS_SUCCESS;
            DoTraceMessage(FLAG_LOG, "%s ACS Support enabled", DEVICE_NAME);
        }
        goto done;
    }

done:
    DoTraceMessage(FLAG_LOG, "%s Cmd_GetFeature GUID: %!GUID! returns with code: 0x%x",
                    DEVICE_NAME, &Guid, status);
    return status;
}

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

    if (!pDevExt)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s Cmd_SetExtFeature pDevExt is NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }
    UNREFERENCED_PARAMETER(feature);
    UNREFERENCED_PARAMETER(Value);
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    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, "[ERROR] %s SNSR_EXT_FEATURE_EOF KeWaitForSingleObject fail, (status 0x%x)",
                    DEVICE_NAME, status);
                KeSetEvent(&pDevExt->ExpEvent, IO_NO_INCREMENT, FALSE);
                break;
            }

            DoTraceMessage(FLAG_LOG, "%s Cmd_SetExtFeature SNSR_EXT_FEATURE_EOF, During Frame=%d", 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");
                    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");
                        pDevExt->remainTimeExp = pDevExt->DefaultDelay;
                    }
                    else
                    {
                        LONG deltaTime;
                        DoTraceMessage(FLAG_LOG, "Calc delay: Use Default, large exposure status");
                        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 ", 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
    case SNSR_EXT_FEATURE_AESETTING_TOCVCHIP:
        status = SetAEToCVChip(pDevExt, *((PAE_SETTING)Value));
        break;
    default:
        status = STATUS_NOT_SUPPORTED;
        break;
    }
    DoTraceMessage(FLAG_LOG, "%s Cmd_SetExtFeature (feature=%d) (0x%Ix)returns with code: 0x%x", SENSOR_NAME,
        feature, Value, status);
    return status;
}

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

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

    if (!pDevExt)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_SetPlatformInfo pDevExt NULL", DEVICE_NAME);
        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)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_SetPos pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    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)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_GetPos pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    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)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_GetStatus pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    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)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_GetHPStatus pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }
    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    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)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_SetConfig pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }

    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    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)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_GetNvmData pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }

    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    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)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_NvmWrite pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }

    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    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)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_NvmRead pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }

    CHECK_DEVICE_HANDLE(pDevExt->DeviceHandle)
    if (!pDevExt->SsNvm.GetEEPRom)
        return STATUS_NOT_SUPPORTED;

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

#if defined(SKY_CAM) || defined(OTM_SENSOR_CONTROL_LOGIC)
NTSTATUS Cmd_GetControlLogicInfo(
    PDEVICE_CONTEXT deviceCtx,
    PVOID info
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    NT_ASSERT(deviceCtx);
    NT_ASSERT(info);
    if (!deviceCtx)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]Cmd_GetControlLogicInfo pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }

    CHECK_DEVICE_HANDLE(deviceCtx->DeviceHandle)
    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 ResIndexRemapping(
    PDEVICE_CONTEXT SensorCtx,
    SNSR_MODE mode,
    INT32 width,
    INT32 height,
    INT32 frameRate,
    INT32 *idx
)
{
    BOOL ismapped = FALSE;
    INT32 RES_NUM = (INT32)ExportResolutionSet[mode].Count;

    if (!SensorCtx)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]ResIndexRemapping pDevExt NULL", DEVICE_NAME);
        return;
    }
    for (INT32 i = 0; i < RES_NUM; i++)
    {
        SENSOR_MODE_TYPE currentmode = Sensor_Res[mode][i].ExtendOpt[RES_EXTEND_ADVANCEDMODE];
        //Map to actual sensor setting index by: width && height && AdvancedSnrmode
        DoTraceMessage(FLAG_LOG, "Current setting[%d] = %dx%d with mode[%d]",
            i, Sensor_Res[mode][i].Width, Sensor_Res[mode][i].Height, currentmode);
        if (currentmode == SensorCtx->AdvancedSnrMode
            && Sensor_Res[mode][i].Width == (ULONG)width
            && Sensor_Res[mode][i].Height == (ULONG)height
            && Sensor_Res[mode][i].FPS == (LONG)frameRate)
        {
            *idx = i;
            ismapped = TRUE;
            break;
        }
    }
    DoTraceMessage(FLAG_LOG, "Remap result is %d, Sensor setting[%d] advanced mode %d",
        ismapped, *idx, SensorCtx->AdvancedSnrMode);
}


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;

    if (!deviceObject)
    {
        return STATUS_INVALID_PARAMETER;
    }

    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, "[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);
    if (!deviceCtx)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]CleanUpWMIFlipModeInterface pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }
    //DoTraceMessage(FLAG_LOG, "%s CleanUpSensorInterfaceContext-- in", DEVICE_NAME);

    status = KeWaitForSingleObject(&deviceCtx->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %s CleanUpSensorInterfaceContext KeWaitForSingleObject fail, (status 0x%x)",
            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",
        SENSOR_NAME, status);

    return status;

}

// For HDMI/DP-In Sensor only, requested by audio driver.
void NotifyAudioJackState(PDEVICE_CONTEXT deviceCtx)
{
    if (deviceCtx && deviceCtx->AudioInterface && deviceCtx->AudioInterface->HdmiI2sSetJackState)
    {
        TraceWPPLogFn(FLAG_LOG, "%s Prepare to send notification to Audio Driver.", DEVICE_NAME);
        deviceCtx->AudioInterface->HdmiI2sSetJackState(deviceCtx->AudioInterface->InterfaceHeader.Context, (UCHAR)(deviceCtx->ControlLogicInfo.ControlLogicID), (UCHAR)FALSE);
        TraceWPPLogFn(FLAG_LOG, "%s controlLogicID = %u, Notification: AudioJackStatus = 0 has sent to audio driver.", DEVICE_NAME, (UCHAR)(deviceCtx->ControlLogicInfo.ControlLogicID));
    }
    else
    {
        DoTraceMessage(FLAG_LOG, "%s Invalid Context to Notify Audio Jack State Or Non HDMI/DP-In Sensor Case!", DEVICE_NAME);
        return;
    }
}

NTSTATUS CleanupSensorCallbackContext(PDEVICE_CONTEXT deviceCtx)
{
    NTSTATUS status = STATUS_SUCCESS;
    if (!deviceCtx || deviceCtx->ControlLogicInterface == NULL)
        return status;

    if (deviceCtx->ControlLogicInterface->SetCallback)
    {
        SENSOR_CB_HDMI_STATUS_UPDATE hdmiUpdate;
        hdmiUpdate.Callback = NULL;
        hdmiUpdate.Context = NULL;
        status = deviceCtx->ControlLogicInterface->SetCallback(deviceCtx->ControlLogicInterface->InterfaceHeader.Context, SENSOR_CALLBACK_HDMI_STATUS_CHANGE, (ULONG_PTR)&hdmiUpdate);
    }

    return status;
}

NTSTATUS QueryCVFInterface(PDEVICE_OBJECT DeviceObject, void **Intf)
{
    PAGED_CODE();
    if (!Intf)
        return STATUS_INVALID_PARAMETER;
    PIRP               irp;
    IO_STATUS_BLOCK    ioStatus;
    KEVENT             event;
    NTSTATUS           status;
    PIO_STACK_LOCATION irpStack = NULL;
    USHORT             irpSize;
    USHORT             irpVersion;
    const GUID              *guid;
    /*
     * steps:
     * 1) Try xsensor driver interface, go done if succeed.
     * 2) Try CVS interafce version 12, go done if succeed.
     */
    *Intf = ExAllocatePool2(POOL_FLAG_NON_PAGED, sizeof(XSENSOR_DRV_INTERFACE), POOL_TAG);
    if (!(*Intf))
        return STATUS_INSUFFICIENT_RESOURCES;
    guid = &GUID_DEVINTERFACE_XSENSOR;
    irpSize = sizeof(XSENSOR_DRV_INTERFACE);
    irpVersion = 0xff;
    do
    {
        KeInitializeEvent(&event, NotificationEvent, FALSE);
        irp = IoBuildSynchronousFsdRequest(IRP_MJ_PNP, DeviceObject, NULL, 0, NULL, &event, &ioStatus);
        if (irp == NULL)
        {
            status = STATUS_INSUFFICIENT_RESOURCES;
            goto invalid;
        }
        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 = guid;
        irpStack->Parameters.QueryInterface.Interface = (PINTERFACE)(*Intf);
        irpStack->Parameters.QueryInterface.Size = irpSize;
        irpStack->Parameters.QueryInterface.Version = irpVersion;
        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 (irpVersion == VISION_INTERFACE_VERSION_12)
            {
                status = STATUS_NOT_SUPPORTED;
                DoTraceMessage(FLAG_ERROR, "[ERROR] Invalid CVF interface, ver:%d size:%d", irpVersion, irpSize);
                goto invalid;
            }
            else // NOLINT
            {
                ExFreePoolWithTag(*Intf, POOL_TAG);
                *Intf = ExAllocatePool2(POOL_FLAG_NON_PAGED, sizeof(VISION_INTERFACE_CAMERA_VER_12), POOL_TAG);
                if (!(*Intf))
                {
                    status = STATUS_INSUFFICIENT_RESOURCES;
                    goto done;
                }
                guid = &GUID_DEVINTERFACE_VISION_CAMERA_API;
                irpVersion = VISION_INTERFACE_VERSION_12;
                irpSize = sizeof(VISION_INTERFACE_CAMERA_VER_12);

                DoTraceMessage(FLAG_ERROR, "try CVF interface ver:%d size:%d", irpVersion, irpSize);
                continue;
            }
        }
        else
        {
            DoTraceMessage(FLAG_LOG,
                           "Get CVF interface ver:%d size:%d successfully.",
                           ((PINTERFACE)(*Intf))->Version,
                           ((PINTERFACE)(*Intf))->Size);
            goto done;
        }
    } while (TRUE);
invalid:
    if (*Intf)
    {
        ExFreePoolWithTag(*Intf, POOL_TAG);
        *Intf = NULL;
    }
done:
    return status;
}

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

    if (deviceCtx->CVFInterface)
    {
        if (((PINTERFACE)(deviceCtx->CVFInterface))->Version > VISION_INTERFACE_VERSION_12)
        {
            ExFreePoolWithTag((PXSENSOR_DRV_INTERFACE)(deviceCtx->CVFInterface), POOL_TAG);
        }
        else
        {
            ExFreePoolWithTag((PVISION_INTERFACE_CAMERA_VER_12)(deviceCtx->CVFInterface), POOL_TAG);
        }
        deviceCtx->CVFInterface = NULL;
    }

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

    DoTraceMessage(FLAG_LOG, "%s CleanUpCVFInterfaceContext returns with code: 0x%x",
        SENSOR_NAME, status);

    return status;
}

NTSTATUS AcquireVisionSensor(PDEVICE_CONTEXT DeviceCtx)
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(DeviceCtx)
    CHECK_DEVICE_HANDLE(DeviceCtx->DeviceHandle)

    status = SensorCtxLock(DeviceCtx);
    CHECK_STATUS(status);

    if (DeviceCtx->CVFInterface)
    {
        if (DeviceCtx->SensorCaps.CVFSupport)
        {
            // If this sensor support CVF, do not use WMI interface for privacy mode.
            CleanUpWMIPrivacyModeInterface(DeviceCtx);
        }
        if (DeviceCtx->CVFPowerOn)
            goto exit;
    }
    else
    {
        goto exit;
    }

    DoTraceMessage(FLAG_LOG, "%s CVF AcquireSensor +", DEVICE_NAME);
    if (((PINTERFACE)(DeviceCtx->CVFInterface))->Version > VISION_INTERFACE_VERSION_12)
    {
        status = ((PXSENSOR_DRV_INTERFACE)(DeviceCtx->CVFInterface))->control(((PINTERFACE)(DeviceCtx->CVFInterface))->Context, XSD_ACQUIRE_SENSOR, NULL);
    }
    else if (VISION_INTERFACE_VERSION_12 == ((PINTERFACE)(DeviceCtx->CVFInterface))->Version
            && ((PVISION_INTERFACE_CAMERA_VER_12)(DeviceCtx->CVFInterface))->acquireSensor)
    {
        VISION_ACQUIRE_SENSOR_INFO_VER_12 visionAcquireInfo;
        memset(&visionAcquireInfo, 0, sizeof(visionAcquireInfo));

        ULONG MipiClk = 0;
        Cmd_GetExtFeature(DeviceCtx, SNSR_EXT_FEATURE_MIPICLK, &MipiClk);
        visionAcquireInfo.mipiConfig.Frequency = MipiClk / 200000;  //Unit is 100Khz
        visionAcquireInfo.mipiConfig.Lanes = DeviceCtx->SensorMipiConfig.MipiLane;
        if (DeviceCtx->SensorDriverView.SensorDriverSubView.Platform >= PLATFORM_LNL)
        {
            visionAcquireInfo.privacyStateCallBack = NULL;
        }
        else
        {
            visionAcquireInfo.privacyStateCallBack = OnCVFPrivacyNotification;
        }

        visionAcquireInfo.cameraContext = DeviceCtx;
        status = ((PVISION_INTERFACE_CAMERA_VER_12)(DeviceCtx->CVFInterface))->acquireSensor(((PINTERFACE)(DeviceCtx->CVFInterface))->Context, &visionAcquireInfo);

        if (DeviceCtx->CameraSetPrivacyMode &&
            DeviceCtx->SensorDriverView.SensorDriverSubView.Platform < PLATFORM_LNL)
        {
            DeviceCtx->CameraSetPrivacyMode(DeviceCtx->CameraContext, visionAcquireInfo.privacy);
            // Do not set resolution when start streaming with privacy ON
            // Need to set resolution during recovery process of exiting privacy flow
            DeviceCtx->bRecoverResolution = visionAcquireInfo.privacy;
        }
    }
    else
    {
        status = STATUS_NOT_SUPPORTED;
    }
    DoTraceMessage(FLAG_LOG, "%s CVF AcquireSensor  - status: 0x%x", DEVICE_NAME, status);

    if (NT_SUCCESS(status))
    {
        DeviceCtx->CVFPowerOn = TRUE;
        DoTraceMessage(FLAG_LOG, "%s CVF AcquireSensor deviceCtx->CVFPowerOn = %d ", DEVICE_NAME, DeviceCtx->CVFPowerOn);
    }

exit:
    SensorCtxUnlock(DeviceCtx);
done:
    return status;
}

NTSTATUS ReleaseVisionSensor(PDEVICE_CONTEXT DeviceCtx)
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(DeviceCtx)
    CHECK_DEVICE_HANDLE(DeviceCtx->DeviceHandle)

    status = SensorCtxLock(DeviceCtx);
    CHECK_STATUS(status);

    if (DeviceCtx->CVFInterface)
    {
        if (!DeviceCtx->CVFPowerOn)
            goto exit;
    }
    else
    {
        goto exit;
    }

    DoTraceMessage(FLAG_LOG, "%s CVF ReleaseSensor +", DEVICE_NAME);
    if (((PINTERFACE)(DeviceCtx->CVFInterface))->Version > VISION_INTERFACE_VERSION_12)
    {
        status = ((PXSENSOR_DRV_INTERFACE)(DeviceCtx->CVFInterface))->control(((PXSENSOR_DRV_INTERFACE)(DeviceCtx->CVFInterface))->interfaceHeader.Context, XSD_RELEASE_SENSOR, NULL);

    }
    else if (VISION_INTERFACE_VERSION_12 == ((PINTERFACE)(DeviceCtx->CVFInterface))->Version
            && ((PVISION_INTERFACE_CAMERA_VER_12)(DeviceCtx->CVFInterface))->releaseSensor)
    {
        status = ((PVISION_INTERFACE_CAMERA_VER_12)(DeviceCtx->CVFInterface))->releaseSensor(((PVISION_INTERFACE_CAMERA_VER_12)(DeviceCtx->CVFInterface))->interfaceHeader.Context);
    }
    else
    {
        status = STATUS_NOT_SUPPORTED;
    }
    DoTraceMessage(FLAG_LOG, "%s CVF ReleaseSensor - status: 0x%x", DEVICE_NAME, status);

    if (NT_SUCCESS(status))
    {
        DeviceCtx->CVFPowerOn = FALSE;
    }

exit:
    SensorCtxUnlock(DeviceCtx);
done:
    return status;
}

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, "[ERROR] %s", szBuf);
    }
}

NTSTATUS RegisterForWMIPrivacyModeNotification(PDEVICE_CONTEXT DeviceCtx,
    GUID WMIGuid, WMI_NOTIFICATION_CALLBACK Callback)
{
    PAGED_CODE();
    NTSTATUS status = STATUS_SUCCESS;

    DoTraceMessage(FLAG_LOG, "%!FUNC!+, %s", SENSOR_NAME);

    status = IoWMIOpenBlock(
        &WMIGuid,
        WMIGUID_NOTIFICATION,
        &DeviceCtx->WMIPrivacyModeNotificationObject
    );

    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%!FUNC!, %s, [ERROR] Unable to open wmi data block status 0x%x\n", SENSOR_NAME, status);
        DeviceCtx->WMIPrivacyModeNotificationObject = NULL;
    }
    else
    {
        status = IoWMISetNotificationCallback(DeviceCtx->WMIPrivacyModeNotificationObject, Callback, DeviceCtx);

        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%!FUNC!, %s, [ERROR] Unable to register for wmi notification 0x%x\n", SENSOR_NAME, status);
            ObDereferenceObject(DeviceCtx->WMIPrivacyModeNotificationObject);
            DeviceCtx->WMIPrivacyModeNotificationObject = NULL;
        }
    }

    DoTraceMessage(FLAG_LOG, "%!FUNC!-, %s", SENSOR_NAME);

    return status;
}

VOID UnregisterForWMIPrivacyModeNotification(PDEVICE_CONTEXT DeviceCtx)
{
    PAGED_CODE();

    DoTraceMessage(FLAG_LOG, "%!FUNC!+, %s", SENSOR_NAME);

    if (DeviceCtx->WMIPrivacyModeNotificationObject != NULL)
    {
        ObDereferenceObject(DeviceCtx->WMIPrivacyModeNotificationObject);
        DeviceCtx->WMIPrivacyModeNotificationObject = NULL;
    }

    DoTraceMessage(FLAG_LOG, "%!FUNC!-, %s", SENSOR_NAME);
}

VOID WMIPrivacyNotificationCallbackForOEM(PVOID Wnode, PVOID Context)
/*

Routine Description:

    WMI calls this function to notify that the privacy mode switch event has occurred.

Arguments:

    Wnode - points to the WNODE_EVENT_ITEM structure returned by the driver triggering the event,
                    which contains the privacy mode status.

    Context - points to the value specified in the Context parameter of the
                    IoWMISetNotificationCallback routine.

Return Value:

    NT status

*/
{
    PWNODE_SINGLE_INSTANCE  wnode = (PWNODE_SINGLE_INSTANCE)Wnode;
    POEM_PRIVACY_STATUS privacyStatus = WDF_PTR_ADD_OFFSET(wnode, wnode->DataBlockOffset);
    PDEVICE_CONTEXT deviceCtx = Context;

    DoTraceMessage(FLAG_LOG, "%!FUNC!+, %s", SENSOR_NAME);
    DoTraceMessage(FLAG_LOG, "%!FUNC!, %s, bufferlength=%u eventtype=0x%x subtype=0x%x scancode=0x%x status=0x%x",
        SENSOR_NAME, privacyStatus->BufferLength, privacyStatus->EventType, privacyStatus->SubType,
        privacyStatus->ChangeCode, privacyStatus->Status);

    if (privacyStatus->BufferLength < PRIVACY_STATUS_BUFFER_LENGTH)
    {
        DoTraceMessage(FLAG_ERROR, "%!FUNC!, %s, [ERROR] buffer length of privacy status is less than %d",
            SENSOR_NAME, PRIVACY_STATUS_BUFFER_LENGTH);
        goto done;
    }

    if (privacyStatus->EventType == PRIVACY_EVENT_TYPE && privacyStatus->SubType == PRIVACY_EVENT_SUBTYPE)
    {
        if (privacyStatus->ChangeCode != CHANGE_CODE_CAMERA_MUTE)
        {
            DoTraceMessage(FLAG_LOG, "%!FUNC!, sensor %s ignore non camera mute change code", SENSOR_NAME);
            goto done;
        }

        if (deviceCtx->CameraSetPrivacyMode)
        {
            // bit1 of status indicates the status of camera privacy mode: 0 - Muted, 1 - Unmuted
            BOOL sts = !(privacyStatus->Status & PRIVACY_EVENT_CAMERA_STATUS);
            if (!deviceCtx->CameraSetPrivacyMode(deviceCtx->CameraContext, sts))
            {
                DoTraceMessage(FLAG_ERROR, "%!FUNC!, [ERROR] sensor %s set privacy mode failed", SENSOR_NAME);
            }
        }
        else
        {
            DoTraceMessage(FLAG_ERROR, "%!FUNC!, [ERROR] sensor %s doesn't have privacy mode callback", SENSOR_NAME);
        }
    }
    else
    {
        DoTraceMessage(FLAG_LOG, "%!FUNC!, sensor %s ignore non privacy notification", SENSOR_NAME);
    }

done:
    DoTraceMessage(FLAG_LOG, "%!FUNC!-, %s", SENSOR_NAME);
}


NTSTATUS WMIQueryPrivacyStateForOEM(PVOID Context, PVOID PrivacyState)
{
    NTSTATUS                status = STATUS_SUCCESS;
    GUID                    wmiGuid = GUID_OEM_DEVICE_STATE;
    PVOID                   buffer = NULL;
    ULONG                   bufferSize = 0;
    PDEVICE_CONTEXT         DeviceCtx = Context;

    PAGED_CODE();
    DoTraceMessage(FLAG_LOG, "%!FUNC!+, %s", SENSOR_NAME);

    if (Context == NULL || PrivacyState == NULL)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC! NULL pointer parameters\n");
        status = STATUS_INVALID_PARAMETER;
        goto done;
    }

    status = IoWMIOpenBlock(&wmiGuid, WMIGUID_QUERY, &DeviceCtx->WMIDeviceStateQueryObject);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC! fail to open wmi data block, status 0x%x\n", status);
        DeviceCtx->WMIDeviceStateQueryObject = NULL;
    }
    else
    {
        bufferSize = sizeof(WNODE_ALL_DATA) + sizeof(OEM_DEVICE_STATE);
        DoTraceMessage(FLAG_LOG, "%!FUNC! try to allocate memory for %d bytes WMI data block buffer\n", bufferSize);
        buffer = ExAllocatePool2(POOL_FLAG_NON_PAGED, bufferSize, POOL_TAG);
        if (!buffer)
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC! failed to allocate memory for %d bytes buffer\n", bufferSize);
            status = STATUS_INSUFFICIENT_RESOURCES;
            goto done;
        }
        RtlSecureZeroMemory(buffer, bufferSize);

        status = IoWMIQueryAllData(DeviceCtx->WMIDeviceStateQueryObject, &bufferSize, buffer);
        if (status == STATUS_BUFFER_TOO_SMALL)
        {
            DoTraceMessage(FLAG_LOG, "%!FUNC! buffer is too small, need to reallocate a %d bytes buffer\n", bufferSize);
            ExFreePoolWithTag(buffer, POOL_TAG);
            buffer = ExAllocatePool2(POOL_FLAG_NON_PAGED, bufferSize, POOL_TAG);
            if (!buffer)
            {
                DoTraceMessage(FLAG_LOG, "[ERROR] %!FUNC! failed to allocate memory for %d bytes buffer\n", bufferSize);
                status = STATUS_INSUFFICIENT_RESOURCES;
                goto done;
            }
            RtlSecureZeroMemory(buffer, bufferSize);
        }

        status = IoWMIQueryAllData(DeviceCtx->WMIDeviceStateQueryObject, &bufferSize, buffer);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC! sensor %s unable to query device state, bufferSize=%ld status=0x%x\n",
                SENSOR_NAME, bufferSize, status);
            DeviceCtx->WMIDeviceStateQueryObject = NULL;
        }
        else
        {
            PWNODE_ALL_DATA wnode = (PWNODE_ALL_DATA)buffer;
            POEM_DEVICE_STATE state = WDF_PTR_ADD_OFFSET(wnode, wnode->DataBlockOffset);
            DoTraceMessage(FLAG_LOG, "%!FUNC! sensor %s query device state successfully", SENSOR_NAME);
            DoTraceMessage(FLAG_LOG, "%!FUNC! BufferSize=%ld DevicesSupported=0x%x CurrentState=0x%x",
                bufferSize, state->DevicesSupported, state->CurrentState);

            if (state->DevicesSupported & DEVICE_STATE_CAMERA_MASK)
            {
                // CurrentState bit1 == 0 == device off == muted == privacy on, so *PrivacyState = 1
                // CurrentState bit1 == 1 == device on == un-muted == privacy off, so *PrivacyState = 0
                *(PULONG)PrivacyState = (state->CurrentState & DEVICE_STATE_CAMERA_MASK) == 0;
            }
            else
            {
                *(PULONG)PrivacyState = 0;
                DoTraceMessage(FLAG_LOG, "%!FUNC! sensor %s doesn't support privacy mode", SENSOR_NAME);
            }
        }
    }

done:
    if (buffer)
    {
        ExFreePoolWithTag(buffer, POOL_TAG);
    }

    if (DeviceCtx && DeviceCtx->WMIDeviceStateQueryObject)
    {
        ObDereferenceObject(DeviceCtx->WMIDeviceStateQueryObject);
        DeviceCtx->WMIDeviceStateQueryObject = NULL;
    }
    DoTraceMessage(FLAG_LOG, "%!FUNC!-, %s", SENSOR_NAME);
    return status;
}
NTSTATUS
GetModuleTableFromRegistry(
    PDEVICE_CONTEXT deviceCtx
)
{
    NTSTATUS status = STATUS_SUCCESS;
    if (deviceCtx == NULL)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC! NULL pointer parameters\n");
        status = STATUS_INVALID_PARAMETER;
        goto done;
    }
    PUCHAR value = (PUCHAR)"Module_Table";
    ULONG bufferLength = 0;

    //First call gets length of value
    status = OS_ReadRegistryValueSize(&g_RegistryPath, value, REG_MULTI_SZ, &bufferLength, POOL_TAG);
    if (!NT_SUCCESS(status) && status != STATUS_BUFFER_TOO_SMALL)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] ReadRegistry: Failed with status = %x", status);
        goto done;
    }
    PUCHAR buffer = (PUCHAR)ExAllocatePool2(
        POOL_FLAG_NON_PAGED, bufferLength, POOL_TAG);

    if (buffer == NULL)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] ModuleName: Failed to allocate space for buffer to store Module names");
        goto done;
    }

    memset(buffer, 0, bufferLength);
    status = OS_ReadRegistry_V2(&g_RegistryPath, value, REG_MULTI_SZ, buffer, &bufferLength, POOL_TAG, deviceCtx);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] ReadRegistry: Failed with status = %x", status);
        goto cleanup;
    }

    ULONG ModuleNameLen = 0;
    INT ModuleIndex = 0;


    //Iterate over each null-terminated string in buffer
    for (PWCHAR strModule = (PWCHAR)buffer; strModule < (PWCHAR)(buffer + bufferLength - 1); strModule += (ModuleNameLen + 1))
    {
        ModuleNameLen = (ULONG)wcsnlen_s(strModule, FILENAME_MAX);
        UNICODE_STRING UnicodeString;
        RtlInitUnicodeString(&UnicodeString, strModule);
        ANSI_STRING AnsiString;
        status = RtlUnicodeStringToAnsiString(&AnsiString, &UnicodeString, TRUE);
        if (!NT_SUCCESS(status))
        {
            RtlFreeAnsiString(&AnsiString);
            DoTraceMessage(FLAG_ERROR, "[ERROR] RtlUnicodeStringToAnsiString: Failed with status = %x", status);
            goto cleanup;
        }
        RtlStringCbCopyA(deviceCtx->ModuleNameTable[ModuleIndex].ModuleName, STRING_LENGTH, AnsiString.Buffer);
        RtlFreeAnsiString(&AnsiString);
        DoTraceMessage(FLAG_LOG, "%!FUNC!, ModuleName %s ", deviceCtx->ModuleNameTable[ModuleIndex].ModuleName);
        ModuleIndex++;
    }

cleanup:
    ExFreePoolWithTag(buffer, POOL_TAG);
done:
    return status;
}

VOID CleanUpWMIPrivacyModeInterface(PDEVICE_CONTEXT DeviceCtx)
{
    if (!DeviceCtx)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]CleanUpWMIPrivacyModeInterface pDevExt NULL", DEVICE_NAME);
        return;
    }
    UnregisterForWMIPrivacyModeNotification(DeviceCtx);
    DeviceCtx->QueryPrivacyState = NULL;
}

VOID CleanUpWMIFlipModeInterface(PDEVICE_CONTEXT DeviceCtx)
{
    if (!DeviceCtx)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]CleanUpWMIFlipModeInterface pDevExt NULL", DEVICE_NAME);
        return;
    }
    UnregisterForWMIFlipAngleNotification(DeviceCtx);
    DeviceCtx->QueryCurrentAngle = NULL;
}

NTSTATUS RegisterForWMIFlipAngleNotification(PDEVICE_CONTEXT DeviceCtx,
    GUID WMIGuid, WMI_NOTIFICATION_CALLBACK Callback)
{
    PAGED_CODE();
    NTSTATUS status = STATUS_SUCCESS;
    if (!DeviceCtx)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]RegisterForWMIFlipAngleNotification pDevExt NULL", DEVICE_NAME);
        return STATUS_INVALID_PARAMETER;
    }

    DoTraceMessage(FLAG_LOG, "%!FUNC!+, %s", SENSOR_NAME);

    status = IoWMIOpenBlock(
        &WMIGuid,
        WMIGUID_NOTIFICATION,
        &DeviceCtx->WMIFlipModeNotificationObject
    );

    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "%!FUNC!, %s, [ERROR] Unable to open wmi data block status 0x%x\n", SENSOR_NAME, status);
        DeviceCtx->WMIFlipModeNotificationObject = NULL;
    }
    else
    {
        status = IoWMISetNotificationCallback(DeviceCtx->WMIFlipModeNotificationObject, Callback, DeviceCtx);

        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "%!FUNC!, %s, [ERROR] Unable to register for wmi notification 0x%x\n", SENSOR_NAME, status);
            ObDereferenceObject(DeviceCtx->WMIFlipModeNotificationObject);
            DeviceCtx->WMIFlipModeNotificationObject = NULL;
        }
    }

    DoTraceMessage(FLAG_LOG, "%!FUNC!-, %s", SENSOR_NAME);

    return status;
}

VOID UnregisterForWMIFlipAngleNotification(PDEVICE_CONTEXT DeviceCtx)
{
    PAGED_CODE();

    DoTraceMessage(FLAG_LOG, "%!FUNC!+, %s", SENSOR_NAME);
    if (!DeviceCtx)
    {
        DoTraceMessage(FLAG_LOG, "%s [ERROR]UnregisterForWMIFlipAngleNotification pDevExt NULL", DEVICE_NAME);
        return;
    }

    if (DeviceCtx->WMIFlipModeNotificationObject != NULL)
    {
        ObDereferenceObject(DeviceCtx->WMIFlipModeNotificationObject);
        DeviceCtx->WMIFlipModeNotificationObject = NULL;
    }

    DoTraceMessage(FLAG_LOG, "%!FUNC!-, %s", SENSOR_NAME);
}

VOID WMIFlipAngleNotificationCallbackForOEM(PVOID Wnode, PVOID Context)
/*

Routine Description:

    WMI calls this function to notify that the angle for flipping changes.

Arguments:

    Wnode - points to the WNODE_EVENT_ITEM structure returned by the driver triggering the event,
                    which contains the angle.

    Context - points to the value specified in the Context parameter of the
                    IoWMISetNotificationCallback routine.

Return Value:

    NT status

*/
{
    PWNODE_SINGLE_INSTANCE  wnode = (PWNODE_SINGLE_INSTANCE)Wnode;
    POEM_FLIP_ANGLE AngleForFlip = WDF_PTR_ADD_OFFSET(wnode, wnode->DataBlockOffset);
    PDEVICE_CONTEXT deviceCtx = Context;

    DoTraceMessage(FLAG_LOG, "%!FUNC!+");

    if (AngleForFlip->Angle > MAX_ANGLE_FOR_EVENT)
    {
        DoTraceMessage(FLAG_ERROR, "%!FUNC!, [ERROR] Invalid Angle inforamtion");
        goto done;
    }
    else
    {
        if (deviceCtx->CameraSetFlipMode)
        {
            if (!deviceCtx->CameraSetFlipMode(deviceCtx->CameraContext, AngleForFlip->Angle))
            {
                DoTraceMessage(FLAG_ERROR, "%!FUNC!, [ERROR] Enable Flip mode failed");
            }
        }
        DoTraceMessage(FLAG_LOG, "%!FUNC!, Ignore non flip related notification");
    }

done:
    DoTraceMessage(FLAG_LOG, "%!FUNC!-");
}

NTSTATUS WMIQueryCurAngleForOEM(PVOID Context, UINT *CurAngle)
{
    NTSTATUS                status = STATUS_SUCCESS;
    GUID                    wmiGuid = GUID_OEM_CURANGLE_WMI_EVENT;
    PDEVICE_CONTEXT         DeviceCtx = Context;
    PVOID                   buffer = NULL;
    ULONG                   bufferSize = 0;
    PAGED_CODE();
    DoTraceMessage(FLAG_LOG, "%!FUNC!+");

    if (Context == NULL || CurAngle == NULL)
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC! NULL pointer parameters\n");
        status = STATUS_INVALID_PARAMETER;
        goto done;
    }

    status = IoWMIOpenBlock(&wmiGuid, WMIGUID_QUERY, &DeviceCtx->WMICurAngleQueryObject);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC! fail to open wmi data block, status 0x%x\n", status);
        DeviceCtx->WMICurAngleQueryObject = NULL;
    }
    else
    {
        bufferSize = sizeof(WNODE_ALL_DATA) + sizeof(OEM_FLIP_ANGLE);
        DoTraceMessage(FLAG_LOG, "%!FUNC! try to allocate memory for %d bytes WMI data block buffer\n", bufferSize);
        buffer = ExAllocatePool2(POOL_FLAG_NON_PAGED, bufferSize, POOL_TAG);
        if (!buffer)
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC! failed to allocate memory for %d bytes buffer\n", bufferSize);
            status = STATUS_INSUFFICIENT_RESOURCES;
            goto done;
        }
        RtlSecureZeroMemory(buffer, bufferSize);

        status = IoWMIQueryAllData(DeviceCtx->WMICurAngleQueryObject, &bufferSize, buffer);
        if (status == STATUS_BUFFER_TOO_SMALL)
        {
            DoTraceMessage(FLAG_LOG, "%!FUNC! buffer is too small, need to reallocate a %d bytes buffer\n", bufferSize);
            ExFreePoolWithTag(buffer, POOL_TAG);
            buffer = ExAllocatePool2(POOL_FLAG_NON_PAGED, bufferSize, POOL_TAG);
            if (!buffer)
            {
                DoTraceMessage(FLAG_LOG, "[ERROR] %!FUNC! failed to allocate memory for %d bytes buffer\n", bufferSize);
                status = STATUS_INSUFFICIENT_RESOURCES;
                goto done;
            }
            RtlSecureZeroMemory(buffer, bufferSize);
        }

        status = IoWMIQueryAllData(DeviceCtx->WMICurAngleQueryObject, &bufferSize, buffer);
        if (!NT_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %!FUNC! sensor %s unable to query device state, bufferSize=%ld status=0x%x\n",
                SENSOR_NAME, bufferSize, status);
            DeviceCtx->WMIDeviceStateQueryObject = NULL;
        }
        else
        {
            PWNODE_ALL_DATA wnode = (PWNODE_ALL_DATA)buffer;
            POEM_FLIP_ANGLE CurAngleFromWMI = WDF_PTR_ADD_OFFSET(wnode, wnode->DataBlockOffset);
            DoTraceMessage(FLAG_LOG, "%!FUNC! sensor %s query device state successfully", SENSOR_NAME);
            DoTraceMessage(FLAG_LOG, "%!FUNC! BufferSize=%ld Angle=0x%x", bufferSize, CurAngleFromWMI->Angle);

            if (CurAngleFromWMI->Angle <= 360)
            {
                *CurAngle = CurAngleFromWMI->Angle;
            }
            else
            {
                DoTraceMessage(FLAG_LOG, "%!FUNC! Invalid angle!");
            }
        }
    }

done:
    if (buffer)
    {
        ExFreePoolWithTag(buffer, POOL_TAG);
    }

    if (DeviceCtx && DeviceCtx->WMICurAngleQueryObject)
    {
        ObDereferenceObject(DeviceCtx->WMICurAngleQueryObject);
        DeviceCtx->WMICurAngleQueryObject = NULL;
    }
    DoTraceMessage(FLAG_LOG, "%!FUNC!-");
    return status;
}

BOOL MatchModuleNameInList(PDEVICE_CONTEXT DeviceCtx, const char **ModuleList)
{
    if (DeviceCtx == NULL)
    {
        return FALSE;
    }

    for (UINT8 i = 0; ModuleList[i] != NULL; i++)
    {
        if (_stricmp((CHAR *)DeviceCtx->DsdtSettings.ModuleName, ModuleList[i]) == 0)
        {
            return TRUE;
        }
    }

    return FALSE;
}

NTSTATUS CVFPowerCheck(PDEVICE_CONTEXT DeviceContext)
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(DeviceContext);

    if (DeviceContext->CVFInterface)
    {
        if (!DeviceContext->CVFPowerOn)
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s CVF support, but CVF is not power on yet", DEVICE_NAME);
            status = STATUS_DEVICE_NOT_READY;
        }
    }

done:
    return status;
}

NTSTATUS SensorCtxLock(PDEVICE_CONTEXT DeviceContext)
{
    NTSTATUS status = STATUS_SUCCESS;
    CHECK_POINTER(DeviceContext);

    const KIRQL irql = KeGetCurrentIrql();
    if (irql <= APC_LEVEL)
    {
        status = KeWaitForSingleObject(&DeviceContext->SettingEvent, Executive, KernelMode, FALSE, NULL);
        if (!CHECK_STATUS_SUCCESS(status))
        {
            DoTraceMessage(FLAG_ERROR, "[ERROR] %s KeWaitForSingleObject fail, (status 0x%x)", DEVICE_NAME, status);
            return status;
        }
    }
done:
    return status;
}

VOID SensorCtxUnlock(PDEVICE_CONTEXT DeviceContext)
{
    if (DeviceContext == NULL)
    {
        return;
    }

    const KIRQL irql = KeGetCurrentIrql();
    if (irql <= DISPATCH_LEVEL)
    {
        KeSetEvent(&DeviceContext->SettingEvent, IO_NO_INCREMENT, FALSE);
    }
}
