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

#include "SensorTrace.h"
#include "SsCommon.h"
#include "platf_info.h"
#include "SsSensor.h"
#include "SsSensor.tmh"
#include "Sensor.h"

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

INT32 g_GuidMask = 0;
INT32 g_MaxInstanceNum = 1;

static void write_i2c_type(UINT8* buf, ULONG data, DATA_TOKE_TYPE type)
{
    if (!buf)
    {
        return;
    }

    switch (type)
    {
    case DATA_8BITS:
        buf[0] = (UINT8)data;
        break;
    case DATA_16BITS:
        buf[0] = (UINT8)(data >> 8);
        buf[1] = (UINT8)(data >> 0);
        break;
    case DATA_24BITS:
        buf[0] = (UINT8)(data >> 16);
        buf[1] = (UINT8)(data >> 8);
        buf[2] = (UINT8)(data >> 0);
        break;
    case DATA_32BITS:
        buf[0] = (UINT8)(data >> 24);
        buf[1] = (UINT8)(data >> 16);
        buf[2] = (UINT8)(data >> 8);
        buf[3] = (UINT8)(data >> 0);
        break;
    default:
        return;
    }
}

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

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

    return FALSE;
}


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

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

    status = i2cWrite(&SensorCtx->I2cDev, Addr, Data, DataBits);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[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 RegWriteExt(
    PDEVICE_CONTEXT SensorCtx,
    ULONG Addr,
    ULONG Data,
    DATA_TOKE_TYPE DataBits,
    ADDR_TOKE_TYPE AddrBits,
    ULONG I2CIndex
    )
{
    NTSTATUS  status = STATUS_SUCCESS;

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

    status = i2cWriteExt(&SensorCtx->I2cDev, Addr, Data, DataBits, AddrBits, I2CIndex);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[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;
    }

    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;
        }
    }
    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);
done:
    return status;
}

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

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

    status = i2cReadExt(&SensorCtx->I2cDev, Addr, Data, DataBits, AddrBits, I2CIndex);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[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->I2cDev.I2c[I2C_GENERAL].AddrBits != ADDR_16BITS)
        goto done;

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

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

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

done:
    return status;
}

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

    CHECK_POINTER(SensorCtx)
    CHECK_POINTER(regs)

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

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


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

    CHECK_POINTER(SensorCtx)
    CHECK_POINTER(regs)

    while(p->Type != DATA_TOKE_TERM)
    {
        if (p->Type >= DATA_8BITS && p->Type <= DATA_32BITS)
        {
            index = 0;
            if (AddrBits == ADDR_8BITS)
            {
                wbuf[index++] = (UINT8)p->Reg;
            }
            else
            {
                wbuf[index++] = (UINT8)(p->Reg >> 8);
                wbuf[index++] = (UINT8)p->Reg;
            }

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

    UNREFERENCED_PARAMETER(deviceCtx);

    for (index = 0; index < MaxNum; index ++)
    {
        pTable[index].Width = pRes[index].Width;
        pTable[index].Height = pRes[index].Height;
        pTable[index].FPS = pRes[index].FPS;
        //pTable[index].BayerOrder = (deviceCtx->Orientation == DEGREE_180) ? pRes[index].BayerOrder180Degree : pRes[index].BayerOrder;
        pTable[index].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 SsInitExportResTable(PDEVICE_CONTEXT deviceCtx)
{
    NTSTATUS status = STATUS_SUCCESS;
    INT32 ResNum, mode, i;
    INT32 MaxNum[maxMode] = {RES_IMAGE_MAX, RES_VIDEO_MAX, RES_IMAGE_MAX, RES_BAYER_MAX};
    const char* PrintString[maxMode] = {"Still", "Video", "Preview", "Raw"};
    SNSR_RESOLUTION* ResExport[maxMode] = {ExportStill, ExportVideo, ExportPreview, ExportRaw};

    if (!deviceCtx)
    {
        return STATUS_INVALID_PARAMETER;
    }

    for (mode = 0; mode < maxMode; mode ++)
    {
        for (i = 0; i <= RES_BAYER_MAX; i ++)
        {
            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)


    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 = LocalCmd_SetExposure(pDevExt, ExposureTime);
    }
invalid:
    // End Mutex
    KeSetEvent(&pDevExt->ExpEvent, IO_NO_INCREMENT, FALSE);
done:
    DoTraceMessage(FLAG_LOG,
        "%s aeCmd_SetExposure: Coarse (%d), AnalogGain (%d) DigitalGain (%d) During Frame %d,  For Frame %d", 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
    )
{
    deviceCtx->SensorInterface.SetExposure              = aeCmd_SetExposure;
    // risk to overun prevous special SexExtFeature for certain sensor driver
    deviceCtx->SensorInterface.SetExtFeature            = Cmd_SetExtFeature;

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

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

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

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

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


NTSTATUS SsPowerGpio(
    PDEVICE_CONTEXT pDevExt,
    BOOLEAN PowerOn
    )
{
    if (!pDevExt)
    {
        return STATUS_INVALID_PARAMETER;
    }

    NTSTATUS status = STATUS_SUCCESS;
    PGPIO_CONTEXT pGpioDev = &(pDevExt->GpioDev);

    BOOLEAN IsRstPulse = pGpioDev->Gpio[GPIO_PWDN].Flag ? TRUE : FALSE;

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

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

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

    if (PowerOn)
    {
        // Standby
        if (pGpioDev->Gpio[GPIO_STANDBY].Flag)
        {
            status = SsGpioPinSet(pGpioDev, GPIO_STANDBY, 1 - pGpioDev->Gpio[GPIO_STANDBY].ActiveLevel);
            if (!NT_SUCCESS(status))
            {
                DoTraceMessage(FLAG_ERROR, "[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 NTSTATUS GetDsdtSettings(
    IN PDEVICE_CONTEXT pDevExt
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    ULONG value = 0;
    ULONG count = 0;
    ULONG i;
    if (!pDevExt)
    {
        return STATUS_INVALID_PARAMETER;
    }

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

    //Get SSDB buffer from BIOS
    NTSTATUS statusACPI = GetAcpiSsdbData(pDevExt->DeviceHandle,
    POOL_TAG,
    sizeof(pDevExt->SensorDriverView),
    &pDevExt->SensorDriverView);
    if (!(NT_SUCCESS(statusACPI)))
    {
        DoTraceMessage(FLAG_ERROR, "[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);
        }

    }

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

    //Rom type
    //WA for LKF FPGA + KBL RVP as the bios menu do not support the new EEPROM
    if (pDevExt->PlatformInfo.hwType == HW_FPGA)
    {
        const char *SensorName = DEVICE_NAME;
        if ((_stricmp(SensorName, "IMX351") == 0) && (_stricmp((CHAR*)pDevExt->DsdtSettings.ModuleName, "F16N02B") == 0
                                                    || _stricmp((CHAR*)pDevExt->DsdtSettings.ModuleName, "F16N02BPO") == 0))
            pDevExt->SensorDriverView.SensorDriverSubView.ROMType = 0x11;//CAT24C64
    }
    //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 = 0x13;//NVM_DW9808
    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;
        DoTraceMessage(FLAG_LOG, "%s Sensoreee 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_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;
    }
    memcpy_s(&SensorCtx->SensorCaps, sizeof(SNSR_CAPS), &DefaultSensorCaps, sizeof(SNSR_CAPS));
    memcpy_s(&SensorCtx->SensorMipiConfig, sizeof(SNSR_MIPI_CONFIG), &DefaultSensorMipiConfig, sizeof(SNSR_MIPI_CONFIG));

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

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

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

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

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

    // orientation, RS4 and after using orientation bits PLDB block below not useful now
    if (SensorCtx->SensorDriverView.SensorDriverSubView.Degree == 180)
        SensorCtx->Orientation = DEGREE_180;
    else
        SensorCtx->Orientation = SensorCtx->SensorDriverView.SensorDriverSubView.Degree;

    DoTraceMessage(FLAG_LOG, "%s SensorDriverView.SensorDriverSubView.Degree:%d, SensorCtx->Orientation:%d", DEVICE_NAME, 
                                 SensorCtx->SensorDriverView.SensorDriverSubView.Degree, SensorCtx->Orientation);

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

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

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

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

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

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

        default:
            break;

        }

        SensorCtx->SensorConf.Votage_rail = SensorCtx->SensorDriverView.SensorDriverSubView.Voltagerail;

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

    }

    return STATUS_SUCCESS;
}


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

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

    if (!pDevExt)
    {
        return;
    }

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

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

    if (!pDevExt)
    {
        return;
    }

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

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

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

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

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

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

#endif

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

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

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

    InitCameraDriverRegistryFlags(pDevExt);
}

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

    DoTraceMessage(FLAG_LOG, "%s SsInitContextCommon", 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->CameraSetPrivacyMode = NULL;
    deviceCtx->CameraContext = NULL;
    deviceCtx->WMIPrivacyModeNotificationObject = NULL;
    deviceCtx->WMIDeviceStateQueryObject = NULL;
    deviceCtx->QueryPrivacyState = NULL;

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

    if (!NT_SUCCESS(GetDsdtSettings(deviceCtx)))
    {
        DoTraceMessage(FLAG_ERROR, "[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);

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

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

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

    // power off current camera module
    status = SensorPower(pDevExt, FALSE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[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:
    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;
    }

    DoTraceMessage(FLAG_LOG, "%s Startup (enters) ", DEVICE_NAME);

    if (pDevExt->SensorCaps.CVFSupport && 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);
    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;
            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);
                }
            }
        }
        // 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)

#ifdef PIXTER_DEFINE
    UnInitCmos(pDevExt);
#endif
    if (pDevExt->SensorCaps.CVFSupport && 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);
    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)
    DoTraceMessage(FLAG_LOG, "%s Cmd_Idle+", DEVICE_NAME);

    if (pDevExt->SensorCaps.CVFSupport &&
        pDevExt->CVFInterface && pDevExt->CVFInterface->ReleaseSensor)
    {
        if (pDevExt->CVFPowerOn)
        {
            DoTraceMessage(FLAG_LOG, "%s CVF ReleaseSensor +", DEVICE_NAME);
            status = pDevExt->CVFInterface->ReleaseSensor(
                pDevExt->CVFInterface->InterfaceHeader.Context);
            DoTraceMessage(FLAG_LOG, "%s CVF ReleaseSensor - status: 0x%x", DEVICE_NAME, status);
            if (NT_SUCCESS(status))
                pDevExt->CVFPowerOn = FALSE;
            else
                goto done;
        }
    }

    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)

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

    ULONG MipiClk = 0;

    if (pDevExt->SensorCaps.CVFSupport &&
        pDevExt->CVFInterface && pDevExt->CVFInterface->AcquireSensor)
    {
        // If this sensor support CVF, do not use WMI interface for privacy mode.
        CleanUpWMIPrivacyModeInterface(pDevExt);

        if (!pDevExt->CVFPowerOn)
        {
            CVF_ACQUIRE_SENSOR_INFO_VER_12 CvfAcquireInfo = { 0 };
            Cmd_GetExtFeature(pDevExt, SNSR_EXT_FEATURE_MIPICLK, &MipiClk);

            CvfAcquireInfo.CameraContext = pDevExt;
            CvfAcquireInfo.PrivacyStateCallBack = OnCVFPrivacyNotification;
            CvfAcquireInfo.MipiConfig.Frequency = MipiClk/200000;  //Unit is 100Khz
            CvfAcquireInfo.MipiConfig.Lanes = pDevExt->SensorMipiConfig.MipiLane;

            DoTraceMessage(FLAG_LOG, "%s CVF AcquireSensor + Frequency = %d, lanes =%d",
                DEVICE_NAME, CvfAcquireInfo.MipiConfig.Frequency, CvfAcquireInfo.MipiConfig.Lanes);
            status = pDevExt->CVFInterface->AcquireSensor(
                pDevExt->CVFInterface->InterfaceHeader.Context,
                &CvfAcquireInfo);
            DoTraceMessage(FLAG_LOG, "%s CVF AcquireSensor  - status: 0x%x", DEVICE_NAME, status);

            if (NT_SUCCESS(status))
            {
                if (pDevExt->CameraSetPrivacyMode)
                    pDevExt->CameraSetPrivacyMode(pDevExt->CameraContext, CvfAcquireInfo.Privacy);

                pDevExt->CVFPowerOn = TRUE;
                DoTraceMessage(FLAG_LOG, "%s CVF AcquireSensor  CvfAcquireInfo.Privacy = %d,  pDevExt->CVFPowerOn = %d ",
                    DEVICE_NAME, CvfAcquireInfo.Privacy, pDevExt->CVFPowerOn);
            }
            else
                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);
            }
        }
        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)

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

    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)

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

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

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

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

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

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

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

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

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

    pDevExt->ResIndex = idx;
    pDevExt->Mode = Mode;
    pDevExt->BankId = 0;
    pDevExt->Binning = (UINT8)Sensor_Res[pDevExt->Mode][pDevExt->ResIndex].Binning;
    pDevExt->MipiBps = (INT32)Sensor_Res[pDevExt->Mode][pDevExt->ResIndex].MipiBps;
done:
    DoTraceMessage(FLAG_LOG, "%s SetResolution index = %d ", 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)

    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 = 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_GetSensorInfo(
    PDEVICE_CONTEXT pDevExt,
    PSNSR_INFO pInfo
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(pDevExt)
    CHECK_POINTER(pInfo)

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

    RtlStringCbCopyA(pInfo->cmos, sizeof(pInfo->cmos), DEVICE_NAME);
    RtlStringCbCopyA(pInfo->module, sizeof(pInfo->module), (CHAR*)pDevExt->DsdtSettings.ModuleName);
    if (pDevExt->SensorDriverView.SensorDriverSubView.Platform == PLATFORM_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_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;
    }

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

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


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

    UNREFERENCED_PARAMETER(pDevExt);

    CHECK_POINTER_INVALID(Port)
    CHECK_POINTER_INVALID(LaneNum)
    CHECK_POINTER_INVALID(OutputFormat)

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

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

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

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

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

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

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

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

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

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

NTSTATUS Cmd_GetVersion(
    PDEVICE_CONTEXT pDevExt,
    SNSR_VERSION_INFO *Version
    )
{
    NTSTATUS status = STATUS_SUCCESS;

    CHECK_POINTER(pDevExt)
    CHECK_POINTER(Version)

    status = Detect(pDevExt, Version);
    if (!NT_SUCCESS(status))
        goto done;

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

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

#ifdef SNSR_PLD_WORKAROUND
    UNREFERENCED_PARAMETER(pDevExt);

    memcpy_s(Pld, sizeof(ACPI_PLD_V2_BUFFER), ModulePldInfo, sizeof(ACPI_PLD_V2_BUFFER));
    memcpy_s(&(pDevExt->Pld), sizeof(ACPI_PLD_V2_BUFFER), ModulePldInfo, sizeof(ACPI_PLD_V2_BUFFER));

    DoTraceMessage(FLAG_LOG, "%s Cmd_GetPld done pld=%d", 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)

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

    *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)

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

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

    CHECK_POINTER(SensorCtx)

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

done:
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s Cmd_ByteWriteReg failed 0x%x", 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)

#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)

    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)

#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)

    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)

    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)

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

    // Viewfinder off
    pDevExt->Streaming = FALSE;
    status = Stream(pDevExt, FALSE);
    KeSetEvent(&pDevExt->SettingEvent, IO_NO_INCREMENT, FALSE);
    if (!NT_SUCCESS(status))
    {
        DoTraceMessage(FLAG_LOG, "%s stop viewfinder error! begin to shutdown!", 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)

    switch (feature)
    {
    case SNSR_EXT_FEATURE_MIPICLK:
        //mipi bit rate per lane
        *pValue = pDevExt->MipiBps;
        DoTraceMessage(FLAG_LOG, "%s Cmd_GetExtFeature Mipi Clock = %d,  returns with code: 0x%x", 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;
    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_SetExtFeature(
    PDEVICE_CONTEXT pDevExt,
    SNSR_EXT_FEATURE_ID feature,
    ULONG_PTR Value
    )
{
    NTSTATUS status = STATUS_NOT_SUPPORTED;

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

    switch (feature)
    {
#ifdef AE_EXACT_CONFIG
    case SNSR_EXT_FEATURE_EOF:

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

            // Start Mutex
            status = KeWaitForSingleObject(&pDevExt->ExpEvent, Executive, KernelMode, FALSE, NULL);
            if (status != STATUS_SUCCESS)
            {
                DoTraceMessage(FLAG_ERROR, "[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
    default:
        status = STATUS_NOT_SUPPORTED;
        break;
    }

    return status;
}

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

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

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

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

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

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

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

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

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

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

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

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

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


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

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

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

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

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

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

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

#if defined(OTM_SENSOR_CONTROL_LOGIC)
NTSTATUS Cmd_GetControlLogicInfo(
    PDEVICE_CONTEXT deviceCtx,
    PVOID info
    )
{
    NTSTATUS status = STATUS_SUCCESS;
    NT_ASSERT(deviceCtx);
    NT_ASSERT(info);

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

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

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

    return status;
}
#endif


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

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

    if (irp == NULL)
        return STATUS_INSUFFICIENT_RESOURCES;

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

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

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

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

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

    return status;
}

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

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

    status = KeWaitForSingleObject(&deviceCtx->SettingEvent, Executive, KernelMode, FALSE, NULL);
    if (!CHECK_STATUS_SUCCESS(status))
    {
        DoTraceMessage(FLAG_ERROR, "[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;

}

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

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

    if (irp == NULL)
        return STATUS_INSUFFICIENT_RESOURCES;

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

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

    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 != CVF_CAMERA_INTERFACE_VERSION_12
            || (intf->InterfaceHeader.Size != sizeof(CVF_INTERFACE_CAMERA_VER_12)))
        {
            status = STATUS_UNSUCCESSFUL;
            DoTraceMessage(FLAG_ERROR, "[ERROR] Invalid CVF interface, ver:%d size:%d",
                intf->InterfaceHeader.Version, intf->InterfaceHeader.Size);
        }
        else
        {
            DoTraceMessage(FLAG_LOG, "CVF interface ver:%d size:%d",
                intf->InterfaceHeader.Version, intf->InterfaceHeader.Size);
        }
    }

    return status;
}

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

    if (deviceCtx->CVFInterface)
    {
        ExFreePoolWithTag(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;
}

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 = ExAllocatePoolWithTag(NonPagedPoolNx, 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 = ExAllocatePoolWithTag(NonPagedPoolNx, 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->WMIDeviceStateQueryObject)
    {
        ObDereferenceObject(DeviceCtx->WMIDeviceStateQueryObject);
        DeviceCtx->WMIDeviceStateQueryObject = NULL;
    }
    DoTraceMessage(FLAG_LOG, "%!FUNC!-, %s", SENSOR_NAME);
    return status;
}

VOID CleanUpWMIPrivacyModeInterface(PDEVICE_CONTEXT DeviceCtx)
{
    UnregisterForWMIPrivacyModeNotification(DeviceCtx);
    DeviceCtx->QueryPrivacyState = NULL;
}
