/*! @file Cam_ov7670.c 
*/

#include <string.h>		// memset() need this header
#include "lpc43xx.h"
#include "i2c.h"
#include "AppDefs.h"

#if CAMERA_SEL != CAMERA_OV7670
#error "camera selection is not OV7670! don't compile this module!"
#endif

//! instance of the main control block structure
DS_CamCB SHARERAM s_ccb;

US_RcvBuf SDRAMRAW g_rcvDbBuf;

extern const unsigned int cg_regTabEleCnt;
extern const unsigned short cg_regTab[];
extern unsigned int InitialSensor_I2C(void);;


unsigned int I2C_Slave_ID;



#define I2C_PERIPH_CLK  180000000
#define I2C_TOUT           100000       /* Approx. delay for CPU @ 120MHz     */

#define A_WR      0                     /* Master will write to the I2C       */
#define A_RD      1                     /* Master will read from the I2C      */

/* Clock Control Unit register bits */
#define CCU_CLK_CFG_RUN   (1 << 0)
#define CCU_CLK_CFG_AUTO  (1 << 1)
#define CCU_CLK_STAT_RUN  (1 << 0)

/* LPC43xx Register Bits Definitions */
#define SFSI2C0_SCL_EFP   (1 <<  0)
#define SFSI2C0_SCL_EHD   (1 <<  2)
#define SFSI2C0_SCL_EZI   (1 <<  3)
#define SFSI2C0_SCL_ZIF   (1 <<  7)
#define SFSI2C0_SDA_EFP   (1 <<  8)
#define SFSI2C0_SDA_EHD   (1 << 10)
#define SFSI2C0_SDA_EZI   (1 << 11)
#define SFSI2C0_SDA_ZIF   (1 << 15)

#define I2C_CONCLR_AAC    (1 <<  2)     /* CONCLR:Assert Acknowledge Clear    */
#define I2C_CONCLR_SIC    (1 <<  3)     /* CONCLR:I2C Interrupt Clear Bit     */
#define I2C_CONCLR_STAC   (1 <<  5)     /* CONCLR:Start Flag Clear            */
#define I2C_CONCLR_I2ENC  (1 <<  6)     /* CONCLR:I2C Interface Disable       */

#define I2C_CONSET_AA     (1 <<  2)     /* CONSET:Assert acknowledge flag     */
#define I2C_CONSET_SI     (1 <<  3)     /* CONSET:I2C interrupt flag          */
#define I2C_CONSET_STO    (1 <<  4)     /* CONSET:STOP flag                   */
#define I2C_CONSET_STA    (1 <<  5)     /* CONSET:START flag                  */
#define I2C_CONSET_I2EN   (1 <<  6)     /* CONSET:I2C interface enable        */

#define I2C_STAT_ST       0x08          /* STAT:START has been transmitted    */
#define I2C_STAT_RPTST    0x10          /* STAT:Repeated START transmitted    */
#define I2C_STAT_SLAWA    0x18          /* STAT:SLA+W transmitted and ACKed   */
#define I2C_STAT_SLAWNA   0x20          /* STAT:SLA+W transmitted, no ACK     */
#define I2C_STAT_DTA      0x28          /* Data transmitted, ACK received     */
#define I2C_STAT_DTNA     0x30          /* Data transmitted, no ACK           */
#define I2C_STAT_ALOST    0x38          /* Arbitration lost                   */

#define I2C_STAT_SLARA    0x40          /* STAT:SLA+R transmitted and ACKed   */
#define I2C_STAT_SLARNA   0x48          /* STAT:SLA+R transmitted, no ACK     */
#define I2C_STAT_DRA      0x50          /* Data received, ack returned        */
#define I2C_STAT_DRNA     0x58          /* Data received, not ack returned    */

/*-----------------------------------------------------------------------------
 *      WaitStart:  Wait until START condition transmitted
 *
 * Parameters:  (none)
 *
 * Return:      0   - START condition has been transmitted
 *              1   - No START condition, timeout occured
 *----------------------------------------------------------------------------*/
static uint32_t WaitStart (void) {
  uint32_t i, stat;
  for (i = I2C_TOUT; i; i--) {
    stat = LPC_I2C0->STAT;
    if (stat == I2C_STAT_ST || stat == I2C_STAT_RPTST) {
      return (0);
    }
  }
  return (1);
}

/*-----------------------------------------------------------------------------
 *      WaitStatus:  Wait for specified status code
 *
 * Parameters:  code - status code to wait for
 *
 * Return:      0   - code is set
 *              1   - code not set, timeout occured
 *----------------------------------------------------------------------------*/
static uint32_t WaitStatus (uint32_t code) {
  uint32_t i;
  for (i = I2C_TOUT; i; i--) {
    if (LPC_I2C0->STAT == code) {
      return (0);
    }
  }
  return (1);
}


static uint32_t I2CForOv7670_Init(void) {
	unsigned int i2cInClk;
  /* Connect base clock */
  LPC_CGU->BASE_APB1_CLK = (1    << 11) |
                           (0x09 << 24) ; /* PLL1 is APB1 clock source        */
	i2cInClk = ((LPC_CGU->PLL1_CTRL >> 16 & 0xFF) + 1) * 12000000;
  /* Enable I2C0 peripheral clock */
  LPC_CCU1->CLK_ABP1_I2C0_CFG = CCU_CLK_CFG_AUTO | CCU_CLK_CFG_RUN;
  while (!(LPC_CCU1->CLK_APB1_I2C0_STAT & CCU_CLK_STAT_RUN));

  /* Configure I2C Pins */
  LPC_SCU->SFSI2C0 = SFSI2C0_SDA_EZI | SFSI2C0_SCL_EZI;

  /* Clock rate 360kHz @ APB1 Clock */
  LPC_I2C0->SCLH = i2cInClk / 360000;
  LPC_I2C0->SCLL = i2cInClk / 360000;

  /* Set I2C Operation */
  LPC_I2C0->CONCLR = I2C_CONCLR_I2ENC | I2C_CONCLR_STAC | I2C_CONCLR_SIC | I2C_CONCLR_AAC;
  LPC_I2C0->CONSET = I2C_CONSET_I2EN; 
  return (0);
}

uint32_t I2CForOv7670_Start (void) {
  LPC_I2C0->CONCLR = I2C_CONCLR_SIC;
  LPC_I2C0->CONSET = I2C_CONSET_STA;  
  return (WaitStart());
}


/*-----------------------------------------------------------------------------
 *      I2C_Stop:  Generate stop condition on I2C bus
 *
 * Return:     0 on success, nonzero on error
 *----------------------------------------------------------------------------*/
static uint32_t I2CForOv7670_Stop (void) {
  LPC_I2C0->CONCLR = I2C_CONCLR_STAC;
  LPC_I2C0->CONSET = I2C_CONSET_STO;
  return (0);
}


/*-----------------------------------------------------------------------------
 *      I2C_Write:  Write a byte to I2C interface
 *
 * Return:     0 on success, nonzero on error
 *----------------------------------------------------------------------------*/
static uint32_t I2CForOv7670_Write (uint8_t byte) {
  LPC_I2C0->DAT = byte;
  LPC_I2C0->CONCLR = I2C_CONCLR_SIC | I2C_CONCLR_STAC;
  return (WaitStatus (I2C_STAT_DTA));   /* Wait for Data Tx, ACK Rx           */
}


/*-----------------------------------------------------------------------------
 *      I2C_Read:  Read a byte from I2C interface
 *----------------------------------------------------------------------------*/
static uint32_t I2CForOv7670_Read (uint32_t ack, uint8_t *byte) {
  uint32_t stat;
  if (ack) LPC_I2C0->CONSET = I2C_CONSET_AA;
  else     LPC_I2C0->CONCLR = I2C_CONCLR_AAC;

  LPC_I2C0->CONCLR = I2C_CONCLR_STAC | I2C_CONCLR_SIC;

  stat = (ack) ? (I2C_STAT_DRA) : (I2C_STAT_DRNA);
  if (WaitStatus (stat) == 0) {
    *byte = LPC_I2C0->DAT;
    return (0);
  }
  return (1);
}


uint8_t ov7670_read_reg(uint8_t Addr, uint8_t *Data)
{
	I2CForOv7670_Start ();
	I2CForOv7670_Write(0x42);
//	DelayTicks(100);
	I2CForOv7670_Write(Addr);
	I2CForOv7670_Stop(); 
	
//	DelayTicks(100);
	I2CForOv7670_Start ();
	I2CForOv7670_Write(0x43);
//	DelayTicks(100);
	
	I2CForOv7670_Read(0, Data);
	I2CForOv7670_Stop(); 
	return 1;
}

uint8_t ov7670_write_reg(uint8_t Addr, uint8_t Data)
{
	I2CForOv7670_Start ();
	I2CForOv7670_Write(0x42);
//	DelayTicks(100);
	I2CForOv7670_Write(Addr);	
//	DelayTicks(100);
	I2CForOv7670_Write(Data);
	I2CForOv7670_Stop(); 
	return 1;
}


void OV7670RegInit(void)
{
	ov7670_write_reg(0x3a, 0x04);		// do not understand??
	ov7670_write_reg(0x40, 0xd0);	// seems to be RGB565
	ov7670_write_reg(0x12, 0x14);	// seems to be QVGA, RGB
	ov7670_write_reg(0x32, 0x80);	// HREF edge offset to data output, do not understand?
	ov7670_write_reg(0x17, 0x16);		// ?
	ov7670_write_reg(0x18, 0x04);		// ?
	ov7670_write_reg(0x19, 0x02);		// ?
	ov7670_write_reg(0x1a, 0x7b);		// ?
	ov7670_write_reg(0x03, 0x06);		// ?
	ov7670_write_reg(0x0c, 0x04);		// DCW enable, ?
	ov7670_write_reg(0x3e, 0x00);		// PCLK div = 1
//	ov7670_write_reg(0x3e, 0x09);
	ov7670_write_reg(0x70, 0x3a);		// Horizontal scale factor?
	ov7670_write_reg(0x71, 0x35);		// Vertical scale factor?
	ov7670_write_reg(0x72, 0x11);		// Vertical down sample by 2, Horizontal down dample by 2
	ov7670_write_reg(0x73, 0x00);		// Enable clock div (=1) for DSP scale control
//	ov7670_write_reg(0x73, 0x01);
	ov7670_write_reg(0xa2, 0x02);		// Pixel Clock Delay
	ov7670_write_reg(0x11, 0x81);		// Internal clock prescalar is 1, ?

	ov7670_write_reg(0x7a, 0x20);		// Gamma Curve Highest Segment Slope
	ov7670_write_reg(0x7b, 0x1c);		// Gamma Curve
	ov7670_write_reg(0x7c, 0x28);
	ov7670_write_reg(0x7d, 0x3c);
	ov7670_write_reg(0x7e, 0x55);
	ov7670_write_reg(0x7f, 0x68);
	ov7670_write_reg(0x80, 0x76);
	ov7670_write_reg(0x81, 0x80);
	ov7670_write_reg(0x82, 0x88);
	ov7670_write_reg(0x83, 0x8f);
	ov7670_write_reg(0x84, 0x96);
	ov7670_write_reg(0x85, 0xa3);
	ov7670_write_reg(0x86, 0xaf);
	ov7670_write_reg(0x87, 0xc4);
	ov7670_write_reg(0x88, 0xd7);
	ov7670_write_reg(0x89, 0xe8);
	
	ov7670_write_reg(0x13, 0xe0);		// ?
	ov7670_write_reg(0x00, 0x00);		// AGC gain = 0?
	
	ov7670_write_reg(0x10, 0x00);		// exposure value = 0
	ov7670_write_reg(0x0d, 0x00);		// Average option is full window
	ov7670_write_reg(0x14, 0x28);		// Automatic gain ceiling
	ov7670_write_reg(0xa5, 0x05);		// ?
	ov7670_write_reg(0xab, 0x07);		// ?
	ov7670_write_reg(0x24, 0x75);
	ov7670_write_reg(0x25, 0x63);
	ov7670_write_reg(0x26, 0xA5);
	ov7670_write_reg(0x9f, 0x78);
	ov7670_write_reg(0xa0, 0x68);
	ov7670_write_reg(0xa1, 0x03);
	ov7670_write_reg(0xa6, 0xdf);
	ov7670_write_reg(0xa7, 0xdf);
	ov7670_write_reg(0xa8, 0xf0);
	ov7670_write_reg(0xa9, 0x90);
	ov7670_write_reg(0xaa, 0x94);
	ov7670_write_reg(0x13, 0xe5);

	ov7670_write_reg(0x0e, 0x61);
	ov7670_write_reg(0x0f, 0x4b);
	ov7670_write_reg(0x16, 0x02);
	ov7670_write_reg(0x1e, 0x37);
	ov7670_write_reg(0x21, 0x02);		// ?
	ov7670_write_reg(0x22, 0x91);
	ov7670_write_reg(0x29, 0x07);
	ov7670_write_reg(0x33, 0x0b);
	ov7670_write_reg(0x35, 0x0b);
	ov7670_write_reg(0x37, 0x1d);
	ov7670_write_reg(0x38, 0x71);
	ov7670_write_reg(0x39, 0x2a);
	ov7670_write_reg(0x3c, 0x78);		// no href when vsync is low
	ov7670_write_reg(0x4d, 0x40);
	ov7670_write_reg(0x4e, 0x20);
	ov7670_write_reg(0x69, 0x00);
	ov7670_write_reg(0x6b, 0x60);
	//ov7670_write_reg(0x6b, 0x0a);
	ov7670_write_reg(0x74, 0x19);
	ov7670_write_reg(0x8d, 0x4f);
	ov7670_write_reg(0x8e, 0x00);
	ov7670_write_reg(0x8f, 0x00);
	ov7670_write_reg(0x90, 0x00);
	ov7670_write_reg(0x91, 0x00);
	ov7670_write_reg(0x92, 0x00);
	ov7670_write_reg(0x96, 0x00);
	ov7670_write_reg(0x9a, 0x80);
	ov7670_write_reg(0xb0, 0x84);
	ov7670_write_reg(0xb1, 0x0c);
	ov7670_write_reg(0xb2, 0x0e);

	ov7670_write_reg(0xb3, 0x82);
	ov7670_write_reg(0xb8, 0x0a);

	ov7670_write_reg(0x43, 0x14);
	ov7670_write_reg(0x44, 0xf0);
	ov7670_write_reg(0x45, 0x34);
	ov7670_write_reg(0x46, 0x58);
	ov7670_write_reg(0x47, 0x28);
	ov7670_write_reg(0x48, 0x3a);
	ov7670_write_reg(0x59, 0x88);
	ov7670_write_reg(0x5a, 0x88);
	ov7670_write_reg(0x5b, 0x44);
	ov7670_write_reg(0x5c, 0x67);
	ov7670_write_reg(0x5d, 0x49);
	ov7670_write_reg(0x5e, 0x0e);
	ov7670_write_reg(0x64, 0x04);
	ov7670_write_reg(0x65, 0x20);
	ov7670_write_reg(0x66, 0x05);
	ov7670_write_reg(0x94, 0x04);
	ov7670_write_reg(0x95, 0x08);
	ov7670_write_reg(0x6c, 0x0a);
	ov7670_write_reg(0x6d, 0x55);
	ov7670_write_reg(0x6e, 0x11);
	ov7670_write_reg(0x6f, 0x9f);
	ov7670_write_reg(0x6a, 0x40);
	ov7670_write_reg(0x01, 0x40);
	ov7670_write_reg(0x02, 0x40);
	ov7670_write_reg(0x13, 0xe7);
	ov7670_write_reg(0x15, 0x00);	// Freee running PCLK, VSYNC changes on falling edge of PCLK/
	
	
	ov7670_write_reg(0x4f, 0x80);
	ov7670_write_reg(0x50, 0x80);
	ov7670_write_reg(0x51, 0x00);
	ov7670_write_reg(0x52, 0x22);
	ov7670_write_reg(0x53, 0x5e);
	ov7670_write_reg(0x54, 0x80);
	ov7670_write_reg(0x58, 0x9e);
	
	ov7670_write_reg(0x41, 0x08);		// AWB gain enable
	ov7670_write_reg(0x3f, 0x00);
	ov7670_write_reg(0x75, 0x05);
	ov7670_write_reg(0x76, 0xe1);
	ov7670_write_reg(0x4c, 0x00);
	ov7670_write_reg(0x77, 0x01);		// 
	ov7670_write_reg(0x3d, 0xc2);	
	ov7670_write_reg(0x4b, 0x09);		// 
	ov7670_write_reg(0xc9, 0x60);
	ov7670_write_reg(0x41, 0x38);
	ov7670_write_reg(0x56, 0x40);
	
	ov7670_write_reg(0x34, 0x11);		// 
	ov7670_write_reg(0x3b, 0x02); 
								
	ov7670_write_reg(0xa4, 0x89);		// 
	ov7670_write_reg(0x96, 0x00);
	ov7670_write_reg(0x97, 0x30);
	ov7670_write_reg(0x98, 0x20);
	ov7670_write_reg(0x99, 0x30);
	ov7670_write_reg(0x9a, 0x84);
	ov7670_write_reg(0x9b, 0x29);
	ov7670_write_reg(0x9c, 0x03);
	ov7670_write_reg(0x9d, 0x4c);
	ov7670_write_reg(0x9e, 0x3f);
	ov7670_write_reg(0x78, 0x04);
	
	ov7670_write_reg(0x79, 0x01);
	ov7670_write_reg(0xc8, 0xf0);
	ov7670_write_reg(0x79, 0x0f);
	ov7670_write_reg(0xc8, 0x00);
	ov7670_write_reg(0x79, 0x10);
	ov7670_write_reg(0xc8, 0x7e);
	ov7670_write_reg(0x79, 0x0a);
	ov7670_write_reg(0xc8, 0x80);
	ov7670_write_reg(0x79, 0x0b);
	ov7670_write_reg(0xc8, 0x01);
	ov7670_write_reg(0x79, 0x0c);
	ov7670_write_reg(0xc8, 0x0f);
	ov7670_write_reg(0x79, 0x0d);
	ov7670_write_reg(0xc8, 0x20);
	ov7670_write_reg(0x79, 0x09);
	ov7670_write_reg(0xc8, 0x80);
	ov7670_write_reg(0x79, 0x02);
	ov7670_write_reg(0xc8, 0xc0);
	ov7670_write_reg(0x79, 0x03);
	ov7670_write_reg(0xc8, 0x40);
	ov7670_write_reg(0x79, 0x05);
	ov7670_write_reg(0xc8, 0x30);
	ov7670_write_reg(0x79, 0x26); 
	ov7670_write_reg(0x09, 0x00);	

	// ov7670_set_size(0, 0, CAM_CAP_W, CAM_CAP_W);
	ov7670_write_reg(0x0F, BVON(1));		// disable HREF at blank
	#if IS_GRAYSCALE != 0
		ov7670_write_reg(0x12, BVON(4) | BVOFF(2) | BVOFF(0));	// QVGA, YUV422
	#else
		ov7670_write_reg(0x12, BVON(4) | BVON(2) | BVOFF(0));	// QVGA, RGB565
	#endif
	ov7670_write_reg(0x15, BVON(5));	// PCLK does not toggle during horizontal blank
	ov7670_write_reg(0x1E, 0x03 | BVOFF(5)); // no mirror XY, no flip y
	ov7670_write_reg(0x3A, 0);		// YUV, Y first
	ov7670_write_reg(0x3D, BVON(7));		// YUV, Y first	, auto gamma
	ov7670_write_reg(0x3E, 0);	// no divide PCLK!

}

extern volatile unsigned int g_250usTick;
extern volatile unsigned int g_newTickCnt;
/*static*/ void DelayTicks(unsigned int cnt)
{
	unsigned int tick;
	tick = g_250usTick;
	while (g_250usTick - tick < cnt);
}

// Gate MCLK, 0 = Off, 1 = On
void MCLKCtl(unsigned int isOn)
{
	LPC_CGU_Type *pCGU = LPC_CGU;
	if (isOn)
		BVCLR(pCGU->BASE_CGU_OUT1_CLK, 0);		
	else
		BVSET(pCGU->BASE_CGU_OUT1_CLK, 0);
}

// power down sequence
void OV7670PwrDnSeq(void)
{
	CAM_CHIP_ENB_L;
	MCLKCtl(MCLK_OFF);
	CAM_LDO_EN_L;
}

// Power on sequence
void OV7670PwrOnSeq(void)
{
	OV7670PwrDnSeq(); 				// 1st power down camera module
	DelayTicks(MS2TICK(300));
	// power on sequence
	CAM_LDO_EN_H;							// 2nd pull reset high for OV7670
	DelayTicks(30);
	CAM_CHIP_ENB_H;						// 2nd entern power down mode if OV7670
	DelayTicks(MS2TICK(1+10));
	MCLKCtl(MCLK_ON);		// 3nd Open MCLK
	DelayTicks(MS2TICK(1+10));
	CAM_CHIP_ENB_L;						// 3nd enter normal mode if OV7670 
	DelayTicks(MS2TICK(10+10));
}

void VSyncIrqInit(void)
// configure VSYNC Pin Interrupt
{

	
	LPC_GPIO_PIN_INT_Type *pPinInt = LPC_GPIO_PIN_INT;
	LPC_SCU_Type *pSCU = LPC_SCU;
	uint32_t t;
#if PIQ_VSYNC != 6
		#error "VSync Pin IRQ slot number must be 6!"
#endif
	// Program pin int slot 6 to VSYNC pin, we use rising edge

	t = pSCU->PINTSEL1 & BFMSKINV(16, 23);
	
	t |= BFVAL(GPN_VSYNC, 16, 20) | BFVAL(GPT_VSYNC, 21, 23);
	pSCU->PINTSEL1 = t;
	
	pPinInt->ISEL &= BVON(PIQ_VSYNC);	// Edge sensitive
	pPinInt->SIENR = BVON(PIQ_VSYNC);	// En Rising edge int
	// pPinInt->SIENF = BVON(PIQ_VSYNC);	// 不能在PixArt的6180 sensor上使用下降沿中断，因为VSync下降沿与数据同时来，根本来不及处理
	pPinInt->RISE = BVON(PIQ_VSYNC);
	NVIC_SetPriority(PIN_INT6_IRQn, 2);
	NVIC_EnableIRQ(PIN_INT6_IRQn);      //Enable GPIO pin 6 Interrupt
}

extern const int cg_tabCnctOrd[8];
extern void PinInit(void);

void SetupSGPIO()
{
	unsigned int i, idx,  t1;
	LPC_CGU_Type *pCGU = LPC_CGU;
	LPC_CCU1_Type *pCCU1 = LPC_CCU1;
	LPC_SGPIO_Type *pSGP = LPC_SGPIO;
	LPC_RGU_Type *pRGU = LPC_RGU;

	NVIC_DisableIRQ(SGPIO_IINT_IRQn);

	pCCU1->CLK_PERIPH_SGPIO_CFG = 1<<0;
	// pCCU1->CLK_PERIPH_CORE_CFG = 1<<0;

	// SGPIO 				  ClkOn, NoAutoBlock, PLL1
	pCGU->BASE_PERIPH_CLK = 0<<1 | 0<<11		| 9UL<<24;

	
	// reset SGPIO
	//					Rst SGPIO   | keep M0 reset!
	pRGU->RESET_CTRL1 = 1<<25 		| 1UL<<24;
	// wait while reseting
	do
	{
		t1 = pRGU->RESET_ACTIVE_STATUS1 & (1<<25);
	}while (0 == t1);
	
	
	pSGP->GPIO_OENREG &= ~(0xFF | 1UL<<SGP_SGPPXCLK | 1UL<<SGP_SGPHSYNC);	// 8???????, SGPIO9, SGPIO10??????
	// ??8?slice A, I, E, J, C, K, F, L ; ?????8????? ; ????8???,??9?????,??????
	for (i=0; i<8; i++)
	{
		idx = cg_tabCnctOrd[i];
		pSGP->OUT_MUX_CFG[idx] =	
		//	noCare	| GPIO OE	
			0UL<<0 	| 0UL<<4;

		pSGP->SLICE_MUX_CFG[idx] = 
		//	!MatchMd | CapRis	| SlcOrPinClk	| 8bit	 | !InvQlf
			0UL << 0 | 0UL<<1 	| 1UL<<2 		| 3UL<<6 | 0UL<<8;

		pSGP->SGPIO_MUX_CFG[idx] = 
		//	ExtClkEn | Pin8Clk | ClkDis		| Pin9Qlf	| CnctEn	| Cnct8
			1UL<<0	 | 0UL<<1  | 1UL<<5		| 1UL<<7	| 1UL<<11	| 3UL<<12;

		pSGP->POS[idx] = 0;	// ????????????

	}

	// !!! warning: first slice must NOT use "Concact mode", must use external pin!!!
	pSGP->SGPIO_MUX_CFG[pin0Slice] = 
	//	ExtClkEn | Pin8Clk | ClkDis		| Pin9Qlf	| extpin(s) | cnct8
		1UL<<0	 | 0UL<<1  | 1UL<<5		| 1UL<<7	| 0UL<<11	| 3UL<<12;

		pSGP->CTRL_ENABLED = (1UL<<pin0Slice) | (1UL<<pin1Slice) | (1UL<<pin2Slice) | (1UL<<pin3Slice) |
		(1UL<<pin4Slice) | (1UL<<pin5Slice) | 1UL<<pin6Slice | 1UL<<pin7Slice;

	// disable clocks of Slices with HSYNC pin and PXCLK pin
	pSGP->OUT_MUX_CFG[SLC_SGPHSYNC] = 0;
	pSGP->SLICE_MUX_CFG[SLC_SGPHSYNC] = 0;
	pSGP->SGPIO_MUX_CFG[SLC_SGPHSYNC] = 1UL<<5;		// CLkDis	
	pSGP->OUT_MUX_CFG[SLC_SGPPXCLK] = 0;
	pSGP->SLICE_MUX_CFG[SLC_SGPPXCLK] = 0;
	pSGP->SGPIO_MUX_CFG[SLC_SGPPXCLK] = 1UL<<5;		// CLkDis

	pSGP->CLR_EN_1 = SLCP0TO7_MASK;	
	pSGP->SET_EN_1 = 0x0001;					//interrupt when slice 0 switches data register	
	NVIC_SetPriority(SGPIO_IINT_IRQn, 1);		// set to high priority
}


// use CGU_OUT1_CLK to output higher clock : 180/4=45MHz
void CGUOutMCLKInit(void)
{
	DS_PinMux2D *pm = (DS_PinMux2D*) LPC_SCU;		
	#if TEST_MODE != TEST_UNDERRUN
		LPC_CGU->IDIVB_CTRL = 9UL<<24;	// IDIVB use PLL1
		LPC_CGU->IDIVB_CTRL |= (14UL-1)<<2;	// set divider, 180/14 > 12MHz
	#else
		LPC_CGU->IDIVB_CTRL = 6UL<<24;		// IDIVB use XTAL
		LPC_CGU->IDIVB_CTRL |= (2UL-1)<<2;	// set divider, 12/2 = 6MHz
	#endif
	LPC_CGU->BASE_CGU_OUT1_CLK = 13UL<<24;	// use IDIVB
	pm->a2d[SPT_MCLK][SPN_MCLK]	= ALT_MCLK | SPNCFG_OUT_HSPD;	
}

void CamInit(void)
{
	uint8_t id[4];
	memset(&s_ccb, 0, sizeof(s_ccb));
	SysTick_Config(SystemCoreClock/4000-1);/* Generate interrupt each 250us      */
	s_ccb.pRcvDbBuf = &g_rcvDbBuf;
	s_ccb.freeRxBufCnt = IS_TWIN_RXBUF ? 2 : 1;
	s_ccb.rdyRxBufCnt = 0;
	// hardware init starts here
	NVIC_DisableIRQ(SGPIO_IINT_IRQn);	
	CGUOutMCLKInit();
	PinInit();

	I2CForOv7670_Init();
	DelayTicks(100);
	ov7670_read_reg(0x0A, &id[0]);		// 0x76
	ov7670_read_reg(0x0B, &id[1]);		// 0x73
	ov7670_read_reg(0x1C, &id[2]);		// 0x7F
	ov7670_read_reg(0x1D, &id[3]);		// 0xA2
	OV7670RegInit();	
	SetupSGPIO();
	VSyncIrqInit();		// VSync use GPIO
}

/*! @brief refresh for color mode
*/
ERRCODE VideoRefresh(void)
{
	#if IS_GRAYSCALE != 0
		return OV7670RefreshGray();
	#else
		return OV7670RefreshColor();
	#endif
}

