//------------------------------------------------------------------------------
//
//  Copyright (C) 2004-2006,2007, Freescale Semiconductor, Inc. All Rights Reserved.
//  THIS SOURCE CODE, AND ITS USE AND DISTRIBUTION, IS SUBJECT TO THE TERMS
//  AND CONDITIONS OF THE APPLICABLE LICENSE AGREEMENT
//
//------------------------------------------------------------------------------
//
//  File:  bspcsi.c
//
//  Provides BSP-specific configuration routines for the CSI peripheral.
//
//------------------------------------------------------------------------------
#include <windows.h>
#pragma warning(push)
#pragma warning(disable: 4005)
#include "bsp.h"
#include "cameradbg.h"
#pragma warning(pop)
#include "bspcsi.h"
#include "ipu.h"
#include "CsiClass.h"
#include "CameraImagic.h"
#include "CameraMagna.h"
#include "CameraOV2640.h"
#include "i2cbus.h"

#include <ceddk.h>
#include <devload.h>
#include <NKIntr.h>
#include "mxarm11.h"
#include "Pmic_regulator.h"
#include "regs_regulator.h"
#include "csp.h"




//------------------------------------------------------------------------------
// External Functions


//------------------------------------------------------------------------------
// External Variables


//------------------------------------------------------------------------------
// Defines
#define BSP_CSI_FUNCTION_ENTRY() \
    DEBUGMSG(ZONE_FUNCTION, (TEXT("++%s\r\n"), __WFUNCTION__))
#define BSP_CSI_FUNCTION_EXIT() \
    DEBUGMSG(ZONE_FUNCTION, (TEXT("--%s\r\n"), __WFUNCTION__))

#define CAMERA_SENSOR_MODULE    csiSensorId_OV2640 // OmniVision as default Sensor
//#define CAMERA_SENSOR_MODULE    csiSensorId_Imagic8803 // Imagic as default Sensor


//------------------------------------------------------------------------------
// Types


//------------------------------------------------------------------------------
// Global Variables

// Camera in Used - Default iMagic
csiSensorId gSensorInUse = CAMERA_SENSOR_MODULE;


//------------------------------------------------------------------------------
// Local Variables


//------------------------------------------------------------------------------
// Local Functions
BOOL BSPSensorSetClockGatingMode(BOOL);
void BSPResetCamera(void);

//-----------------------------------------------------------------------------
//
// Function:  BSPCSIIOMUXConfig
//
// This function makes the DDK call to configure the IOMUX
// pins required for the CSI.
//
// Parameters:
//      None.
//
// Returns:
//      TRUE if success; FALSE if failure.
//
//-----------------------------------------------------------------------------
BOOL BSPCSIIOMUXConfig()
{
    BSP_CSI_FUNCTION_ENTRY();

    BOOL retVal = TRUE;
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D5, DDK_IOMUX_OUT_GPIO, DDK_IOMUX_IN_NONE))
    {
        retVal = FALSE;
    }
    DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D5, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
    DDKGpioSetConfig(DDK_GPIO_PORT3, 5, DDK_GPIO_DIR_OUT, DDK_GPIO_INTR_NONE);
    DDKGpioWriteDataPin(DDK_GPIO_PORT3, 5, 0);

    //Reset camera sensor MCU2_14, LOW active after power up stably
    BSPResetCamera();

    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D15, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D14, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D13, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D12, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D11, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D10, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D9, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D8, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D7, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D6, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_HSYNC, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_MCLK, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_PIXCLK, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }
    if (!DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_VSYNC, DDK_IOMUX_OUT_FUNC, DDK_IOMUX_IN_FUNC))
    {
        retVal = FALSE;
    }

        DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D15, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D14, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D13, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D12, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D11, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D10, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D9, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D8, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D7, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D6, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_HSYNC, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_MCLK, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_PIXCLK, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_VSYNC, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);

	DDKIomuxSetPinMux(DDK_IOMUX_PIN_GPIO1_6, DDK_IOMUX_OUT_GPIO, DDK_IOMUX_IN_NONE);
	DDKIomuxSetPadConfig(DDK_IOMUX_PAD_GPIO1_6, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
	DDKGpioSetConfig(DDK_GPIO_PORT1, 6, DDK_GPIO_DIR_OUT, DDK_GPIO_INTR_NONE);
	DDKGpioWriteDataPin(DDK_GPIO_PORT1, 6, 1);
    BSP_CSI_FUNCTION_EXIT();

    return retVal;
}


//------------------------------------------------------------------------------
//
// Function: BSPSetupCamera
//
// This function initializes the camera sensor module.
//
// Parameters:
//      None.
//
// Returns:
//      None.
//
//------------------------------------------------------------------------------
void BSPSetupCamera(void)
{
    BSP_CSI_FUNCTION_ENTRY();

    switch (gSensorInUse)
    {

        case csiSensorId_OV2640:
        default:
             DEBUGMSG(ZONE_INIT, (TEXT("setupCamera: OmniVision 2640 sensor initialization\r\n")));
             CameraOV2640Init();        
             break;        
	}

    BSP_CSI_FUNCTION_EXIT();

    return;
}

//------------------------------------------------------------------------------
//
// Function: BSPDeleteCamera
//
// This function deinitializes the camera sensor module.
//
// Parameters:
//      None.
//
// Returns:
//      None.
//
//------------------------------------------------------------------------------
void BSPDeleteCamera(void)
{
    BSP_CSI_FUNCTION_ENTRY();

    switch (gSensorInUse)
    {
        case csiSensorId_Magna521DA:
            CameraMagnaDeinit();
            DEBUGMSG(ZONE_INIT,(TEXT("DeleteCamera: Magna sensor deinitialization\r\n")));
            break;

        case csiSensorId_Imagic8201:
        case csiSensorId_Imagic8803:
            CameraImagicDeinit();
            DEBUGMSG(ZONE_INIT,(TEXT("DeleteCamera: Imagic sensor deinitialization\r\n")));
        case csiSensorId_OV2640:
        default:
             CameraOV2640Deinit();
             DEBUGMSG(ZONE_INIT,(TEXT("DeleteCamera: OmniVision sensor deinitialization\r\n")));
             break;     
    }

    BSP_CSI_FUNCTION_EXIT();

    return;
}

//------------------------------------------------------------------------------
//
// Function: BSPEnableCamera
//
// This function deinitializes the camera sensor module.
//
// Parameters:
//      None.
//
// Returns:
//      None.
//
//------------------------------------------------------------------------------
void BSPEnableCamera(void)
{
    BSP_CSI_FUNCTION_ENTRY();

    // TODO: Skip this if we are using HSP_CLK, otherwise uncomment
    //DDKClockConfigBaud(DDK_CLOCK_SIGNAL_CSI, DDK_CLOCK_BAUD_SOURCE_USBPLL, 0, );
    //Enpower sensor
    {
        //Set camera voltages
        PMIC_REGULATOR_VREG_VOLTAGE voltage;
	  PmicSwitchModeRegulatorSetVoltageLevel (SW2B, SW_VOLTAGE_STBY, 0x24); 
	  PmicSwitchModeRegulatorOn(SW2B);
	  
	  voltage.v_vib = V_VIB_1_3V;
	  PmicVoltageRegulatorSetVoltageLevel(V_VIB, voltage);
	  voltage.vmmc = VMMC_6;
	  PmicVoltageRegulatorSetVoltageLevel(VMMC1, voltage);
	  	  		
	  //Added for VVIB and VMMC1 voltage setting
        PmicVoltageRegulatorOn(V_VIB);
        PmicVoltageRegulatorOn(VMMC1);
	  Sleep(100);
    }

    //reset and pddn disable
    DDKIomuxSetPinMux(DDK_IOMUX_PIN_CSI_D5, DDK_IOMUX_OUT_GPIO, DDK_IOMUX_IN_NONE);
   
    DDKIomuxSetPadConfig(DDK_IOMUX_PAD_CSI_D5, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
    DDKGpioSetConfig(DDK_GPIO_PORT3, 5, DDK_GPIO_DIR_OUT, DDK_GPIO_INTR_NONE);
    DDKGpioWriteDataPin(DDK_GPIO_PORT3, 5, 0);
	Sleep(100);

	//Reset camera sensor MCU3_3, LOW active after power up stably
    BSPResetCamera();

    BSP_CSI_FUNCTION_EXIT();

    return;
}

//------------------------------------------------------------------------------
//
// Function: BSPDisableCamera
//
// This function deinitializes the camera sensor module.
//
// Parameters:
//      None.
//
// Returns:
//      None.
//
//------------------------------------------------------------------------------
void BSPDisableCamera(void)
{
    BSP_CSI_FUNCTION_ENTRY();

    //BSPSensorSetClockGatingMode(FALSE);

    BSP_CSI_FUNCTION_EXIT();

    return;
}

//------------------------------------------------------------------------------
//
// Function: BSPResetCamera
//
// This function reset the camera sensor module.
//
// Parameters:
//      None.
//
// Returns:
//      None.
//
//------------------------------------------------------------------------------
void BSPResetCamera(void)
{
    BSP_CSI_FUNCTION_ENTRY();
    //Reset camera sensor MCU3_3, LOW active after power up stably
    DDKIomuxSetPinMux(DDK_IOMUX_PIN_RI_DTE1, DDK_IOMUX_OUT_GPIO, DDK_IOMUX_IN_GPIO);
    DDKIomuxSetPadConfig(DDK_IOMUX_PAD_RI_DTE1, DDK_IOMUX_PAD_SLEW_SLOW, DDK_IOMUX_PAD_DRIVE_NORMAL, DDK_IOMUX_PAD_MODE_CMOS, DDK_IOMUX_PAD_TRIG_SCHMITT, DDK_IOMUX_PAD_PULL_UP_100K);
    DDKGpioSetConfig(DDK_GPIO_PORT2, 14, DDK_GPIO_DIR_OUT, DDK_GPIO_INTR_NONE);
    DDKGpioWriteDataPin(DDK_GPIO_PORT2, 14, 0);
    Sleep(100);
    DDKGpioWriteDataPin(DDK_GPIO_PORT2, 14, 1); 
    Sleep(100);	  

    BSP_CSI_FUNCTION_EXIT();
    
    return;
}

//------------------------------------------------------------------------------
//
// Function: BSPGetSensorClockRatio
//
// This function calculates the divider value to program into
// the CSI to achieve the required frame rate from the sensor.
//
// Parameters:
//      None.
//
// Returns:
//      Sensor clock divider value.
//
//------------------------------------------------------------------------------
UINT32 BSPGetSensorClockRatio()
{
    UINT32 freq;

    BSP_CSI_FUNCTION_ENTRY();

    DDKClockGetFreq(DDK_CLOCK_SIGNAL_CSI, &freq);

    BSP_CSI_FUNCTION_EXIT();

    // TODO: Change this
    // For a target frame rate of 30 FPS, we want a sensor clock
    // in the 24-27MHz range.
    return (freq/24000000 - 1);
}

//------------------------------------------------------------------------------
//
// Function: BSPGetDefaultCameraFromRegistry
//
// This function reads the default camera sensor from the
// registry.
//
// Parameters:
//      None.
//
// Returns:
//      TRUE if success
//      FALSE if failure
//
//------------------------------------------------------------------------------
BOOL BSPGetDefaultCameraFromRegistry(void)
{
    UINT32 error;
    HKEY hKey;
    UINT32 dwSize;
    csiSensorId sensorId;

    BSP_CSI_FUNCTION_ENTRY();

    // Open CSI registry path
    error = RegOpenKeyEx(HKEY_LOCAL_MACHINE, TEXT(CSI_REG_PATH), 0 , 0, &hKey);
    if (error != ERROR_SUCCESS) {
        DEBUGMSG(ZONE_ERROR,(TEXT("Failed to open camera reg path:%s [Error:0x%x]\r\n"),CSI_REG_PATH,error));
        return (FALSE);
    }

    // Get Default Camera
    dwSize = sizeof(csiSensorId);
    error = RegQueryValueEx(hKey, TEXT(CSI_REG_CAMERAID_KEYWORD), NULL, NULL,(LPBYTE)&sensorId, (LPDWORD)&dwSize);
    if (error == ERROR_SUCCESS)
    {
        // Make sure it's valid CameraID
        if(sensorId >= 0 && sensorId < CSI_SENSOR_NUMBER_SUPPORTED)
        {
            gSensorInUse = sensorId;
        }
        else
        {
            DEBUGMSG(ZONE_ERROR,(TEXT("Invalid Camera ID, set to default:%d\r\n"),CAMERA_SENSOR_MODULE));
        }
    }
    else
    {
        DEBUGMSG(ZONE_ERROR,(TEXT("Failed to get the default CameraID [Error:0x%x]\r\n"),error));
    }

    // Close registry key
    RegCloseKey(hKey);

    BSP_CSI_FUNCTION_EXIT();

    return TRUE;
}

//------------------------------------------------------------------------------
//
// Function: BSPGetSensorFormat
//
// This function returns the sensor data format.
//
// Parameters:
//      pSensorFormat
//          [out] Pointer to DWORD to hold return value of sensor format.
//
// Returns:
//      None.
//
//------------------------------------------------------------------------------
void BSPGetSensorFormat(DWORD *pSensorFormat)
{
    BSP_CSI_FUNCTION_ENTRY();

    *pSensorFormat = csiSensorOutputFormat_YUV422;

    BSP_CSI_FUNCTION_EXIT();	
}


//------------------------------------------------------------------------------
//
// Function: BSPGetSensorResolution
//
// This function returns the sensor data format.
//
// Parameters:
//      pSensorFormat
//          [out] Pointer to DWORD to hold return value of sensor resolution.
//
// Returns:
//      None.
//
//------------------------------------------------------------------------------
void BSPGetSensorResolution(DWORD *pSensorResolution)
{
    BSP_CSI_FUNCTION_ENTRY();

    switch (gSensorInUse)
    {
      case csiSensorId_Imagic8803:
      case csiSensorId_Imagic8201:
           *pSensorResolution = csiSensorOutputResolution_VGA;
           break;
      case csiSensorId_OV2640:
      default:
           *pSensorResolution = csiSensorOutputResolution_VGA; 
           break;

    }  

    BSP_CSI_FUNCTION_EXIT();  

}

//------------------------------------------------------------------------------
//
// Function: BSPSetDigitalZoom
//
// This function sets the zoom value on the camera sensor.
//
// Parameters:
//      zoom
//          [in] If 2, zoom set to 2x; if 1, zoom set to 1x
//
// Returns:
//      None.
//
//------------------------------------------------------------------------------
void BSPSetDigitalZoom (DWORD zoom)
{
    BSP_CSI_FUNCTION_ENTRY();

    switch (gSensorInUse)
    {
      case csiSensorId_Imagic8803:
      case csiSensorId_Imagic8201:
           CameraImagicSetDigitalZoom(zoom);
           break;
 
      case csiSensorId_OV2640:
      default:
           CameraOV2640SetDigitalZoom(zoom);       
           break;
    }        
    BSP_CSI_FUNCTION_EXIT();
}

//------------------------------------------------------------------------------
//
// Function: BSPCameraSetOutputResolution
//
// This function sets the output resolution on the camera sensor.
//
// Parameters:
//      outputMode
//          [in] Resolution type.
//
// Returns:
//      TRUE if success
//      FALSE if failure
//
//------------------------------------------------------------------------------
BOOL BSPCameraSetOutputResolution(csiSensorOutputResolution outputResolution)
{
    BSP_CSI_FUNCTION_ENTRY();

    switch (gSensorInUse)
    {
      case csiSensorId_Imagic8803:
      case csiSensorId_Imagic8201:
           CameraImagicSetOutputResolution(outputResolution);
           break;


      case csiSensorId_OV2640:
      default:
           CameraOV2640SetOutputResolution(outputResolution);
           break;
    }        

    BSP_CSI_FUNCTION_EXIT();

    return TRUE;
}

//------------------------------------------------------------------------------
//
// Function: BSPCameraSetOutputFormat
//
// This function sets the output format on the camera sensor.
//
// Parameters:
//      outputFormat
//          [in] Format specified to output sensor data.
//
// Returns:
//      TRUE if success
//      FALSE if failure
//
//------------------------------------------------------------------------------
BOOL BSPCameraSetOutputFormat(csiSensorOutputFormat outputFormat)
{
    cameraOutputFormat cameraOutputFormat;
    
    BSP_CSI_FUNCTION_ENTRY();

    switch (outputFormat)
    {
      case csiSensorOutputFormat_Bayer:
           return FALSE;

      case csiSensorOutputFormat_YUV422:
           cameraOutputFormat = cameraOutputFormat_CCIR656;
           break;

      case csiSensorOutputFormat_YUV444:
           return FALSE;

      case csiSensorOutputFormat_RGB:
           cameraOutputFormat = cameraOutputFormat_RGB565;
           break;

      default:
           cameraOutputFormat = cameraOutputFormat_RGB565;
           break;
    }

    switch (gSensorInUse)
    {
      case csiSensorId_Imagic8803:
      case csiSensorId_Imagic8201:
           // Call Imagic method to set output format
           CameraImagicSetOutputFormat(cameraOutputFormat);
           break;
		   
      case csiSensorId_OV2640:
      default:
           CameraOV2640SetOutputFormat(cameraOutputFormat);
           break;
    }        
    
    BSP_CSI_FUNCTION_EXIT();

    return TRUE;
}

//------------------------------------------------------------------------------
//
// Function: BSPSensorSetClockGatingMode
//
// This function calls to the CRM module to
// set the clock gating mode, turning on or off
// clocks to the Camera Sesnor.
//
// Parameters:
//      startClocks
//          [in] If TRUE, turn clocks to GPT on.
//                If FALSE, turn clocks to GPT off
//
// Returns:
//      TRUE if success.
//      FALSE if failure.
//
//------------------------------------------------------------------------------
BOOL BSPSensorSetClockGatingMode(BOOL startClocks)
{
    BSP_CSI_FUNCTION_ENTRY();

    if (startClocks)
    {
        // Turn CSI and sensor clocks on
        if (!DDKClockSetGatingMode(DDK_CLOCK_GATE_INDEX_CSI,
            DDK_CLOCK_GATE_MODE_ENABLED_ALL))
        {
            DEBUGMSG(ZONE_ERROR, (TEXT("%s: Failed to set clock gating mode!\r\n"), __WFUNCTION__));
            return FALSE;
        }
    }
    else
    {
        // Turn CSI and sensor clocks off
        if (!DDKClockSetGatingMode(DDK_CLOCK_GATE_INDEX_CSI,
            DDK_CLOCK_GATE_MODE_DISABLED))
        {
            DEBUGMSG(ZONE_ERROR, (TEXT("%s: Failed to set clock gating mode!\r\n"), __WFUNCTION__));
            return FALSE;
        }
    }

    BSP_CSI_FUNCTION_EXIT();

    return TRUE;
}

//-----------------------------------------------------------------------------
//
// Function: I2CWriteOneByte
//
// This function writes a single byte byData to the register stated in byReg.
//
// Parameters:
//      hI2C
//          [in] File handle to I2C Bus Interface.
//
//      byAddr
//          [in] I2C Slave device address.
//
//      byReg
//          [in] Register Index.
//
//      byData
//          [in] Data to write to byReg.
//
//      lpiResult
//          [in] Pointer of the result. The I2C Bus will store the
//            result of the operation in location pointed to by
//            lpiResult.
//
// Returns:
//      None.
//
//-----------------------------------------------------------------------------
VOID I2CWriteOneByte(HANDLE hI2C, BYTE byAddr, BYTE byReg, BYTE byData, LPINT lpiResult)
{
    I2C_TRANSFER_BLOCK I2CXferBlock;
    I2C_PACKET I2CPacket;
    BYTE byOutData[2];

    BSP_CSI_FUNCTION_ENTRY();

    byOutData[0] = byReg;
    byOutData[1] = byData;
    I2CPacket.wLen = sizeof(byOutData);
    I2CPacket.pbyBuf = (PBYTE) &byOutData;

    I2CPacket.byRW = I2C_RW_WRITE;
    I2CPacket.byAddr = byAddr;
    I2CPacket.lpiResult = lpiResult;

    I2CXferBlock.pI2CPackets = &I2CPacket;
    I2CXferBlock.iNumPackets = 1;

    I2C_MACRO_TRANSFER(hI2C, &I2CXferBlock);

    BSP_CSI_FUNCTION_EXIT();
}

//-----------------------------------------------------------------------------
//
// Function: I2CReadOneByte
//
// This function read a single byte data from the register stated in byReg.
//
// Parameters:
//      hI2C
//          [in] File handle to I2C Bus Interface.
//
//      byAddr
//          [in] I2C Slave device address.
//
//      byReg
//          [in] Register Index.
//
//      lpiResult
//          [in] Pointer of the result. The I2C Bus will store the
//            result of the operation in location pointed to by
//            lpiResult.
//
// Returns:
//      The single byte content stored in byReg.
//
//-----------------------------------------------------------------------------
BYTE I2CReadOneByte(HANDLE hI2C, BYTE byAddr, BYTE byReg, LPINT lpiResult)
{
    I2C_TRANSFER_BLOCK I2CXferBlock;
    I2C_PACKET I2CPacket[2];
    BYTE byOutData;
    BYTE byInData;

    BSP_CSI_FUNCTION_ENTRY();

    byOutData = byReg;

    I2CPacket[0].pbyBuf = (PBYTE) &byOutData;
    I2CPacket[0].wLen = sizeof(byOutData);

    I2CPacket[0].byRW = I2C_RW_WRITE;
    I2CPacket[0].byAddr = byAddr;
    I2CPacket[0].lpiResult = lpiResult;


    I2CPacket[1].pbyBuf = (PBYTE) &byInData;
    I2CPacket[1].wLen = sizeof(byInData);

    I2CPacket[1].byRW = I2C_RW_READ;
    I2CPacket[1].byAddr = byAddr;
    I2CPacket[1].lpiResult = lpiResult;

    I2CXferBlock.pI2CPackets = I2CPacket;
    I2CXferBlock.iNumPackets = 2;

    I2C_MACRO_TRANSFER(hI2C, &I2CXferBlock);

    BSP_CSI_FUNCTION_EXIT();

    return byInData;
}

//-----------------------------------------------------------------------------
//
// Function: I2CWriteTwoBytes
//
// This function writes a two byte data to the register stated in byReg. The
// function will write byData1 first, followed by byData2. If byData1 is not
// written properly, the entire function will terminate without attempting to
// write byData2.
//
// Parameters:
//      hI2C
//          [in] File handle to I2C Bus Interface.
//
//      byAddr
//          [in] I2C Slave device address.
//
//      byReg
//          [in] Register Index.
//
//      byData1
//          [in] 1st Data to write to byReg.
//
//      byData2
//          [in] 2nd Data to write to byReg
//
//      lpiResult
//          [in] Pointer of the result. The I2C Bus will store the
//            result of the operation in location pointed to by
//            lpiResult.
//
// Returns:
//      None.
//
//-----------------------------------------------------------------------------
VOID I2CWriteTwoBytes(HANDLE hI2C, BYTE byAddr, BYTE byReg, BYTE byData1, BYTE byData2, LPINT lpiResult)
{
    I2C_TRANSFER_BLOCK I2CXferBlock;
    I2C_PACKET I2CPacket;
    BYTE byOutData[3];

    BSP_CSI_FUNCTION_ENTRY();

    byOutData[0] = byReg;
    byOutData[1] = byData1;
    byOutData[2] = byData2;

    I2CPacket.wLen = sizeof(byOutData);
    I2CPacket.pbyBuf = (PBYTE) &byOutData;

    I2CPacket.byRW = I2C_RW_WRITE;
    I2CPacket.byAddr = byAddr;
    I2CPacket.lpiResult = lpiResult;

    I2CXferBlock.pI2CPackets = &I2CPacket;
    I2CXferBlock.iNumPackets = 1;

    I2C_MACRO_TRANSFER(hI2C, &I2CXferBlock);

    BSP_CSI_FUNCTION_EXIT();
}

//-----------------------------------------------------------------------------
//
// Function: I2CReadTwoBytes
//
// This function reads two bytes of data from the register stated in byReg.
//
// Parameters:
//      hI2C
//          [in] File handle to I2C Bus Interface.
//
//      byAddr
//          [in] I2C Slave device address.
//
//      byReg
//          [in] Register Index.
//
//      lpiResult
//          [in] Pointer of the result. The I2C Bus will store the
//          result of the operation in location pointed to by
//          lpiResult.
//
// Returns:
//      The two byte content stored in byReg.
//
//-----------------------------------------------------------------------------
WORD I2CReadTwoBytes(HANDLE hI2C, BYTE byAddr, BYTE byReg, LPINT lpiResult)
{
    I2C_TRANSFER_BLOCK I2CXferBlock;
    I2C_PACKET I2CPacket[2];
    BYTE byOutData;
    BYTE byInData[2];

    BSP_CSI_FUNCTION_ENTRY();

    byOutData = byReg;

    I2CPacket[0].pbyBuf = (PBYTE) &byOutData;
    I2CPacket[0].wLen = sizeof(byOutData);

    I2CPacket[0].byRW = I2C_RW_WRITE;
    I2CPacket[0].byAddr = byAddr;
    I2CPacket[0].lpiResult = lpiResult;


    I2CPacket[1].pbyBuf = (PBYTE) &byInData;
    I2CPacket[1].wLen = sizeof(byInData);

    I2CPacket[1].byRW = I2C_RW_READ;
    I2CPacket[1].byAddr = byAddr;
    I2CPacket[1].lpiResult = lpiResult;

    I2CXferBlock.pI2CPackets = I2CPacket;
    I2CXferBlock.iNumPackets = 2;

    I2C_MACRO_TRANSFER(hI2C, &I2CXferBlock);

    BSP_CSI_FUNCTION_EXIT();

    return ((0xFF & byInData[1]) | (((WORD) byInData[0]) << 8));
}