/*
-- Company: Trenz Electronic GmbH
-- Engineer: Oleksandr Kiyenko
 */
#include <stdio.h>
#include <fcntl.h>
#include <stdlib.h>
#include <unistd.h>
#include "sensor_config.h"
#include <xiicps.h>

#define IIC_CLK						100000

#define CAMERA_V1_3_IIC_ADDRESS		0x36
#define CAMERA_V2_1_IIC_ADDRESS		0x10

#define CS_CMMN_CHIP_ID_H	   		0x300A
#define CS_CMMN_CHIP_ID_L			0x300B

u8 i2c_reg_read(XIicPs *InstancePtr, u8 chip_addr, u16 reg_addr)
{
	u8 wr_data[2];
	u8 rd_data;

	wr_data[0] = reg_addr >> 8;
	wr_data[1] = reg_addr & 0xFF;
	XIicPs_MasterSendPolled(InstancePtr, wr_data, 2, chip_addr);
	XIicPs_MasterRecvPolled(InstancePtr, &rd_data, 1, chip_addr);
	while (XIicPs_BusIsBusy(InstancePtr));
	return rd_data;
}

int i2c_reg_write(XIicPs *InstancePtr, u8 chip_addr, u16 reg_addr, u8 reg_val){
    u8 WriteBuffer[3];
	int Status;

	WriteBuffer[0] = reg_addr >> 8;
	WriteBuffer[1] = reg_addr & 0xFF;
	WriteBuffer[2] = reg_val;
    Status = XIicPs_MasterSendPolled(InstancePtr, WriteBuffer, 3, chip_addr);
    if (Status != XST_SUCCESS) {
		return Status;
    }

    /* Wait until bus is idle to start another transfer */
    while (XIicPs_BusIsBusy(InstancePtr));

	return XST_SUCCESS;
}

static void i2c_set_write(XIicPs *InstancePtr, unsigned char dev_addr, struct sensor_cmd *set){
	int i;
	for(i=0; set[i].reg != TABLE_END; i++){
		i2c_reg_write(InstancePtr, dev_addr, set[i].reg, set[i].val);
	}
}

int init_iic(XIicPs *InstancePtr){
	XIicPs_Config *Config;
	int Status;

	Config = XIicPs_LookupConfig(XPAR_XIICPS_0_DEVICE_ID);
	if (NULL == Config) {
	   xil_printf("XIicPs_LookupConfig failure\r\n");
	   return XST_FAILURE;
	}

	Status = XIicPs_CfgInitialize(InstancePtr, Config, Config->BaseAddress);
	if (Status != XST_SUCCESS) {
	   xil_printf("XIicPs_CfgInitialize failure\r\n");
	   return XST_FAILURE;
	}

	Status = XIicPs_SelfTest(InstancePtr);
	if (Status != XST_SUCCESS) {
		print("Failure in SelfTest() - ");
		return XST_FAILURE;
	}

	XIicPs_SetSClk(InstancePtr, IIC_CLK);
	return XST_SUCCESS;
}

int main(int argc, char *argv[])
{
	unsigned short model_id;
	unsigned int lot_id;
	unsigned short chip_id;
	unsigned char ret;
	
	printf("Raspberry Pi Camera Init v1.3\n");
	XIicPs InstancePtr; /* The instance of the IIC device. */
	init_iic(&InstancePtr);

	if(!((i2c_reg_read(&InstancePtr, CAMERA_V1_3_IIC_ADDRESS, CS_CMMN_CHIP_ID_H) != 0x56) || (i2c_reg_read(&InstancePtr, CAMERA_V1_3_IIC_ADDRESS, CS_CMMN_CHIP_ID_L) != 0x47))){
		printf("Camera V1.X configuration\n");
		i2c_reg_write(&InstancePtr, CAMERA_V1_3_IIC_ADDRESS, 0x0100, 0x00);					// Disable
		i2c_reg_write(&InstancePtr, CAMERA_V1_3_IIC_ADDRESS, 0x0103, 0x01);		  			// Reset
		usleep(1);																		// Wait
		i2c_reg_write(&InstancePtr, CAMERA_V1_3_IIC_ADDRESS, 0x0103, 0x00);					// Reset
		usleep(10 * 1000);																// Wait
		i2c_set_write(&InstancePtr, CAMERA_V1_3_IIC_ADDRESS, ov5647_sensor_common_10bit);   // Load common configuration
		i2c_set_write(&InstancePtr, CAMERA_V1_3_IIC_ADDRESS, ov5647_sensor_1296_968_30);	// Load specific configuration
		i2c_reg_write(&InstancePtr, CAMERA_V1_3_IIC_ADDRESS, 0x0100, 0x01);		  			// Enable
		printf("Camera init complete.\n");
		return 1;
	}

	printf("Camera V2.X configuration\n");
	ret = i2c_reg_read(&InstancePtr, CAMERA_V2_1_IIC_ADDRESS, 0x0000);
	if (ret < 0) {
		perror("Failure to read Model ID (high byte)\n");
		return 0;
	}
	model_id = ret << 8;
	ret = i2c_reg_read(&InstancePtr, CAMERA_V2_1_IIC_ADDRESS, 0x0001);
	if (ret < 0) {
		perror("Failure to read Model ID (low byte)\n");
		return 0;
	}
	model_id |= ret;
	ret = i2c_reg_read(&InstancePtr, CAMERA_V2_1_IIC_ADDRESS, 0x0004);
	if (ret < 0) {
		perror("Failure to read Lot ID (high byte)\n");
		return 0;
	}
	lot_id = ret << 16;
	ret = i2c_reg_read(&InstancePtr, CAMERA_V2_1_IIC_ADDRESS, 0x0005);
	if (ret < 0) {
		perror("Failure to read Lot ID (mid byte)\n");
		return 0;
	}
	lot_id |= ret << 8;
	ret = i2c_reg_read(&InstancePtr, CAMERA_V2_1_IIC_ADDRESS, 0x0006);
	if (ret < 0) {
		perror("Failure to read Lot ID (low byte)\n");
		return 0;
	}
	lot_id |= ret;
	ret = i2c_reg_read(&InstancePtr, CAMERA_V2_1_IIC_ADDRESS, 0x000D);
	if (ret < 0) {
		perror("Failure to read Chip ID (high byte)\n");
		return 0;
	}
	chip_id = ret << 8;
	ret = i2c_reg_read(&InstancePtr, CAMERA_V2_1_IIC_ADDRESS, 0x000E);
	if (ret < 0) {
		perror("Failure to read Chip ID (low byte)\n");
		return 0;
	}
	chip_id |= ret;
	if (model_id != 0x0219) {
		perror("Model not supported!\n");
		return 0;
	}

	printf("Found 2.X Camera Model ID 0x%04x, Lot ID 0x%06x, Chip ID 0x%04x\n", model_id, lot_id, chip_id);
	i2c_set_write(&InstancePtr, CAMERA_V2_1_IIC_ADDRESS, imx219_720p_regs);
	printf("Camera init complete.\n");
	return 3;
}