#include <bsp.h>
#include <pm.h>
#include "pmplatform.h"
#include "Module.h"
#include "camera_hal.h"
#include "i2c_Camera.h"

// Variables
static MODULE_DESCRIPTOR    gModuleDesc;
static HANDLE               g_hI2C;   // I2C Bus Driver

#define IIC_DRIVER_PORT     _T("I2C1:")//_T("I2C0:") // modify by jerry

// Definitions
#define MSG_ERROR        1

/////////////////////////////////////////////////////////////////////////////////
// copy module data to output buffer
void ModuleGetFormat(MODULE_DESCRIPTOR *outModuleDesc)
{
    memcpy(outModuleDesc, &gModuleDesc, sizeof(MODULE_DESCRIPTOR));
}

int ModuleInit()
{
    DWORD dwErr = ERROR_SUCCESS, bytes;
    UINT32 uiIICDelay;
    DBGMSG(CAMC_FUNC, (TEXT("+ModuleInit\n")));

    g_hI2C = OpenI2C( IIC_DRIVER_PORT );

    InitI2C(g_hI2C, CAMERA_WRITE_ADDRESS, IIC_CLOCK);

    memset(&gModuleDesc , 0 , sizeof(MODULE_DESCRIPTOR));

    gModuleDesc.ITUXXX = DEFAULT_MODULE_ITUXXX;
    gModuleDesc.ModuleInterlace = DEFAULT_MODULE_INTERLACE;
    gModuleDesc.MIPI = DEFAULT_MODULE_MIPI;
    gModuleDesc.UVOffset = DEFAULT_MODULE_UVOFFSET;
    gModuleDesc.SourceHSize = DEFAULT_MODULE_HSIZE;
    gModuleDesc.Order422 = DEFAULT_MODULE_YUVORDER;
    gModuleDesc.SourceVSize = DEFAULT_MODULE_VSIZE;
    gModuleDesc.Clock = DEFAULT_MODULE_CLOCK;
    gModuleDesc.Codec = DEFAULT_MODULE_CODEC;
    gModuleDesc.HighRst = DEFAULT_MODULE_HIGHRST;
    gModuleDesc.SourceHOffset = DEFAULT_MODULE_HOFFSET;
    gModuleDesc.SourceVOffset = DEFAULT_MODULE_VOFFSET;
    gModuleDesc.InvPCLK = DEFAULT_MODULE_INVPCLK;
    gModuleDesc.InvVSYNC = DEFAULT_MODULE_INVVSYNC;
    gModuleDesc.InvHREF = DEFAULT_MODULE_INVHREF;

    DBGMSG(CAMC_FUNC, (_T("gModuleDesc Dump:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n"),
    gModuleDesc.ITUXXX,
    gModuleDesc.ModuleInterlace,
    gModuleDesc.MIPI,
    gModuleDesc.UVOffset,
    gModuleDesc.SourceHSize,
    gModuleDesc.Order422,
    gModuleDesc.SourceVSize,
    gModuleDesc.Clock,
    gModuleDesc.Codec,
    gModuleDesc.HighRst,
    gModuleDesc.SourceHOffset,
    gModuleDesc.SourceVOffset,
    gModuleDesc.InvPCLK,
    gModuleDesc.InvVSYNC,
    gModuleDesc.InvHREF    
    ));
 
    DBGMSG(CAMC_FUNC, (TEXT("-ModuleInit\n")));
    return TRUE;
}

void ModuleDeinit()
{
    CloseHandle(g_hI2C);
    g_hI2C = NULL;

}

// This will write command to Camera Module through I2C interface
int  ModuleWriteBlock()
{
    UINT32 i;
    DWORD nLen=0;
    DMA_ADAPTER_OBJECT Adapter;    
    PBYTE pCommandBuf=NULL;
    PHYSICAL_ADDRESS pPhyCommandBuf;
    BOOL    bRet = TRUE;
    
    nLen = sizeof(MODULE_REGISTER)/sizeof(MODULE_REGISTER[0]);

    DBGMSG(CAMC_FUNC, (TEXT("+%s, CommandSet:0x%x, nLen:%d\n"), _T(__FUNCTION__), MODULE_REGISTER, nLen));

    memset(&Adapter, 0, sizeof(DMA_ADAPTER_OBJECT));
    Adapter.InterfaceType = Internal;
    Adapter.ObjectSize = sizeof(DMA_ADAPTER_OBJECT);

    if(pCommandBuf == NULL)
    {
        pCommandBuf  = (PBYTE)HalAllocateCommonBuffer(&Adapter, nLen, &pPhyCommandBuf, FALSE);
    }

    if (pCommandBuf == NULL)
    {
        ERRMSG( (_T("%s(): Failed to allocate DMA buffer for Command Buffer.\r\n"), _T(__FUNCTION__)));
        return FALSE;
    }

    memcpy(pCommandBuf, MODULE_REGISTER, sizeof(MODULE_REGISTER[0])*nLen);

    if(gModuleDesc.MIPI)
    {   
        // Use 2Byte Command Type(HW, SW) and 2Byte Subaddress or 2byte Data(4byte per IIC Transaction)
        for(i=0; i<nLen; i++)
        {

            //RETAILMSG(FALSE,(TEXT("+%s, %d -> %d\n"), _T(__FUNCTION__), MODULE_REGISTER[i], MODULE_REGISTER[i],
            //    (pCommandBuf+i*sizeof(BYTE)*2), pCommandBuf+i*sizeof(BYTE)+1));
            // This will use 16bit Register Setting data.
            if(((UINT32)MODULE_REGISTER[i] & 0xFFFF0000) == 0x00000000)
            {
                // Wait xxx milliseconds.
                Sleep(((UINT32)MODULE_REGISTER[i] & 0x0000FFFF));
            }
            else
            {
                bRet = HW_WriteRegisters16Bit((UINT32)MODULE_REGISTER[i]);
                if(!bRet)
                {
                    // Stop writing command
                    ERRMSG( (TEXT("There is some connection problem to Camera Module, stop writing command\r\n")));
                    break;
                }
            }
        }
    }
    else        
    {   
        for(i=0; i<nLen; i++)
        {
            // Use Command Register and Data set(2byte per IIC Transaction).
            //RETAILMSG(FALSE,(TEXT("+%s, %d,%d -> %d,%d\n"), _T(__FUNCTION__), MODULE_REGISTER[i][0], MODULE_REGISTER[i][1],
            //    (pCommandBuf+i*sizeof(BYTE)*sizeof(MODULE_REGISTER[0])), pCommandBuf+i*sizeof(BYTE)+1));
                
            bRet = HW_WriteRegisters(pCommandBuf+i*sizeof(BYTE)*sizeof(MODULE_REGISTER[0]), sizeof(MODULE_REGISTER[0]));
            
            if(!bRet)
            {
                // Stop writing command
                ERRMSG( (TEXT("There is some connection problem to Camera Module, stop writing command\r\n")));                
                break;
            }
        }
    }

    if(pCommandBuf != NULL)
    {
        HalFreeCommonBuffer(&Adapter, 0, pPhyCommandBuf, (PVOID)pCommandBuf, FALSE);
    }

    if(!gModuleDesc.MIPI)
    {
        ModuleSetImageSize(MODULE_IMAGE_SIZE);
    }
    
    DBGMSG(CAMC_FUNC, (TEXT("-ModuleWriteBlock\n")));
    /*    
    for(i=0; i<(sizeof(MODULE_REGISTER)/2) && (res == 0); i++)
    {
        HW_ReadRegisters(&BUF, MODULE_REGISTER[i][0], 1);
        RETAILMSG(1,(TEXT("0x%x\n"), BUF));
    }
    */
    return bRet;        
}

int  ModulePrepare()
{
    int bRet=TRUE;
    DBGMSG(CAMC_FUNC, (TEXT("+%s\n"), _T(__FUNCTION__)));
    //do nothing
    DBGMSG(CAMC_FUNC, (TEXT("-%s=%d\n"), _T(__FUNCTION__), bRet));

    return bRet;        
}

int  ModuleStart()
{
    int bRet=TRUE;
	RETAILMSG(1, (TEXT("ModuleStart() 2\r\n")));
    DBGMSG(CAMC_FUNC, (TEXT("+%s\n"), _T(__FUNCTION__)));
    //do nothing
    DBGMSG(CAMC_FUNC, (TEXT("-%s=%d\n"), _T(__FUNCTION__), bRet));
    return bRet;        
}


BOOL
HW_WriteRegisters(
    PUCHAR pBuff,   // Optional buffer
    DWORD nRegs     // number of registers
    )
{
    DWORD dwErr = 0;
    DWORD bytes;        // Read
    WriteI2C_Camera(g_hI2C, pBuff, nRegs );

    if ( dwErr ) {
        ERRMSG( (TEXT("I2CWrite ERROR: %u \r\n"), dwErr));
        return FALSE;
    }            
    DBGMSG(CAMC_FUNC, (TEXT("-HW_WriteRegisters\n")));    

    return TRUE;
}

// This function will use Set file itself.
BOOL
HW_WriteRegisters16Bit(UINT32 setFileValue)
{
    DMA_ADAPTER_OBJECT Adapter;    
    PBYTE pCommandBuf = NULL;
    PHYSICAL_ADDRESS pPhyCommandBuf;
    BOOL bRet;
    
    memset(&Adapter, 0, sizeof(DMA_ADAPTER_OBJECT));
    Adapter.InterfaceType = Internal;
    Adapter.ObjectSize = sizeof(DMA_ADAPTER_OBJECT);

    if(pCommandBuf == NULL)
    {
        pCommandBuf  = (PBYTE)HalAllocateCommonBuffer(&Adapter, 4, &pPhyCommandBuf, FALSE);
    }

    if (pCommandBuf == NULL)
    {
        ERRMSG((_T("%s(): Failed to allocate DMA buffer for Command Buffer.\r\n"), _T(__FUNCTION__)));
        return FALSE;
    }

    // write at Device D000:SubAddr    

    pCommandBuf[0] = ((UCHAR *)&setFileValue)[3];
    pCommandBuf[1] = ((UCHAR *)&setFileValue)[2];
    pCommandBuf[2] = ((UCHAR *)&setFileValue)[1];
    pCommandBuf[3] = ((UCHAR *)&setFileValue)[0];

    bRet = HW_WriteRegisters(pCommandBuf, 4);

    if(pCommandBuf != NULL)
    {
        HalFreeCommonBuffer(&Adapter, 0, pPhyCommandBuf, (PVOID)pCommandBuf, FALSE);
    }
    
    return bRet;
}


DWORD HW_ReadRegisters(
    PUCHAR pBuff,       // Optional buffer
    UCHAR StartReg,     // Start Register
    DWORD nRegs         // Number of Registers
    )
{
    DWORD dwErr=0;
    DWORD bytes;
    ReadI2C_Camera(g_hI2C, &StartReg, 1 , FALSE, pBuff, nRegs);
 
    return dwErr;
}

// S5K4CA Camera module uses this.
BOOL ModuleSetHwRegMIPI(uchar ucHSubAddr, uchar ucLSubAddr, uchar ucHData, uchar ucLData)
{
    DMA_ADAPTER_OBJECT Adapter;    
    PBYTE pCommandBuf = NULL;
    PHYSICAL_ADDRESS pPhyCommandBuf;
    BOOL    bRet = TRUE;
    
    memset(&Adapter, 0, sizeof(DMA_ADAPTER_OBJECT));
    Adapter.InterfaceType = Internal;
    Adapter.ObjectSize = sizeof(DMA_ADAPTER_OBJECT);

    if(pCommandBuf == NULL)
    {
        pCommandBuf  = (PBYTE)HalAllocateCommonBuffer(&Adapter, 4, &pPhyCommandBuf, FALSE);
    }

    if (pCommandBuf == NULL)
    {
        DBGMSG(CAMC_FUNC, (_T("%s(): Failed to allocate DMA buffer for Command Buffer.\r\n"), _T(__FUNCTION__)));
        return FALSE;
    }

    // write at Device D000:SubAddr    

    pCommandBuf[0] = 0x00;  // Page Selection
    pCommandBuf[1] = 0x28;  // Page Selection
    pCommandBuf[2] = PAGE_VALUE1;  // Device Page Address   // HW Register
    pCommandBuf[3] = PAGE_VALUE2;  // Device Page Address

    bRet = HW_WriteRegisters(pCommandBuf, 4);

    pCommandBuf[0] = 0x00;  // Page Selection
    pCommandBuf[1] = 0x2a;  // Page Selection
    pCommandBuf[2] = ucHSubAddr;  // Sub Address
    pCommandBuf[3] = ucLSubAddr;  // Sub Address

    bRet = HW_WriteRegisters(pCommandBuf, 4);    

    pCommandBuf[0] = 0x0f;  // Page Selection
    pCommandBuf[1] = 0x12;  // Page Selection
    pCommandBuf[2] = ucHData;  // Data
    pCommandBuf[3] = ucLData;  // Data

    bRet = HW_WriteRegisters(pCommandBuf, 4);    

    if(pCommandBuf != NULL)
    {
        HalFreeCommonBuffer(&Adapter, 0, pPhyCommandBuf, (PVOID)pCommandBuf, FALSE);
    }
    
    return bRet;
}

// S5K4CA Camera module uses this.
BOOL ModuleSetSwRegMIPI(uchar ucHSubAddr, uchar ucLSubAddr, uchar ucHData, uchar ucLData)
{
    DMA_ADAPTER_OBJECT Adapter;    
    PBYTE pCommandBuf = NULL;
    PHYSICAL_ADDRESS pPhyCommandBuf;
    BOOL    bRet = TRUE;
    
    memset(&Adapter, 0, sizeof(DMA_ADAPTER_OBJECT));
    Adapter.InterfaceType = Internal;
    Adapter.ObjectSize = sizeof(DMA_ADAPTER_OBJECT);

    if(pCommandBuf == NULL)
    {
        pCommandBuf  = (PBYTE)HalAllocateCommonBuffer(&Adapter, 4, &pPhyCommandBuf, FALSE);
    }

    if (pCommandBuf == NULL)
    {
        ERRMSG((_T("%s(): Failed to allocate DMA buffer for Command Buffer.\r\n"), _T(__FUNCTION__)));
        return FALSE;
    }

    // write at Device 7000:SubAddr
    
    pCommandBuf[0] = 0x00;  // Page Selection
    pCommandBuf[1] = 0x28;  // Page Selection
    pCommandBuf[2] = 0x70;  // Device Page Address   // SW Register  
    pCommandBuf[3] = 0x00;  // Device Page Address

    bRet = HW_WriteRegisters(pCommandBuf, 4);

    pCommandBuf[0] = 0x00;  // Page Selection
    pCommandBuf[1] = 0x2a;  // Page Selection
    pCommandBuf[2] = ucHSubAddr;  // Sub Address
    pCommandBuf[3] = ucLSubAddr;  // Sub Address

    bRet = HW_WriteRegisters(pCommandBuf, 4);    

    pCommandBuf[0] = 0x0f;  // Page Selection
    pCommandBuf[1] = 0x12;  // Page Selection
    pCommandBuf[2] = ucHData;  // Data
    pCommandBuf[3] = ucLData;  // Data

    bRet = HW_WriteRegisters(pCommandBuf, 4);    
    
    if(pCommandBuf != NULL)
    {
        HalFreeCommonBuffer(&Adapter, 0, pPhyCommandBuf, (PVOID)pCommandBuf, FALSE);
    }
    
    return bRet;
}

int ModuleSetImageSize(int imageSize)
{
    DMA_ADAPTER_OBJECT Adapter;    
    PBYTE pCommandBuf = NULL;
    PHYSICAL_ADDRESS pPhyCommandBuf;
    BOOL    bRet = TRUE;
    
    memset(&Adapter, 0, sizeof(DMA_ADAPTER_OBJECT));
    Adapter.InterfaceType = Internal;
    Adapter.ObjectSize = sizeof(DMA_ADAPTER_OBJECT);

    if(pCommandBuf == NULL)
    {
        pCommandBuf  = (PBYTE)HalAllocateCommonBuffer(&Adapter, 2, &pPhyCommandBuf, FALSE);
    }

    if (pCommandBuf == NULL)
    {
        ERRMSG((_T("%s(): Failed to allocate DMA buffer for Command Buffer.\r\n"), _T(__FUNCTION__)));
        return FALSE;
    }

    pCommandBuf[0] = PAGE_VALUE1;
    pCommandBuf[1] = PAGE_VALUE2;

    bRet = HW_WriteRegisters(pCommandBuf, 2);

    if(pCommandBuf != NULL)
    {
        HalFreeCommonBuffer(&Adapter, 0, pPhyCommandBuf, (PVOID)pCommandBuf, FALSE);
    }

    bRet = HW_WriteRegisters(SupportedModuleImageSize[imageSize], 2);
    
    return bRet;
}

// Dummy function for old interface code
MODULE_STATUS ModuleSetPower(MODULE_PWR_STATUS NewPower)
{
    MODULE_STATUS ModStatus = MOD_STATUS_SUCCESS;
    
    return ModStatus;
}

void ModuleSelectSensor(MODULE_SENSOR SensorID)    
{
}

int ModuleSetFormatSize(CAMIF_OPERATION_MODE OPMode, CAMIF_IMG_SIZE imageSize)
{
    DBGMSG(CAMC_FUNC, (TEXT("+%s\n"), _T(__FUNCTION__)));
    DBGMSG(CAMC_FUNC, (TEXT("-%s\n"), _T(__FUNCTION__)));

    return MOD_STATUS_SUCCESS;        
}