/* PS Eye test program
   by Jim Paris <jim@jtan.com>
   License: GPLv2 or GPLv3
*/
#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <stdint.h>
#include <err.h>
#include <usb.h>
#include "hexdump.h"

#define debug(x...) fprintf(stderr,x)
#define info(x...) fprintf(stderr,x)

/****** Lowlevel USB stuff ******/

#define LIBUSB_BULK_SIZE 16384

usb_dev_handle *eye = NULL;

/* Close USB device */
void eye_close(void)
{
	if (eye != NULL) {
		usb_close(eye);
		eye = NULL;		
	}
}

/* Open & init USB device */
void eye_open(void)
{
	struct usb_bus *bus;
	struct usb_device *dev;
	struct usb_device *usb_dev = NULL;
	
	usb_init();

	if (usb_find_busses() < 0)
		err(1, "Bus scan");
	if (usb_find_devices() < 0)
		err(1, "Device scan");
	
	for (bus = usb_busses; bus; bus = bus->next) {
		for (dev = bus->devices; dev; dev = dev->next) {
			if(dev->descriptor.idVendor==0x1415 &&
			   dev->descriptor.idProduct==0x2000) {
				usb_dev = dev;
				break;
			}
		}
	}
	if (usb_dev == NULL)
		errx(1, "Camera not found.");
	if ((eye = usb_open(usb_dev)) == NULL)
		errx(1, "Error opening camera device.");
	atexit(eye_close);

	if (usb_claim_interface(eye, 0))
		err(1, "Can't claim camera interface");

	info("Opened device\n");

	return;
}

/* Send the control message used to read/write eye registers.
   Returns 0 on success or negative on error. */
int eye_control_msg(uint16_t reg, uint8_t *value, int is_write)
{
	int ret;
	ret = usb_control_msg(
		eye,
		(is_write ? USB_ENDPOINT_OUT : USB_ENDPOINT_IN) |
		USB_TYPE_VENDOR | USB_RECIP_DEVICE,
		0x01,	// request
		0x0000, // value
		reg,    // index
		(char *)value, 1,
		500);
	if (ret < 0)
		return -1;
	if (ret != 1)
		return -2;
	return 0;
}

/* Do a bulk read on endpoint 1.  Returns read length on success. */
int eye_bulk_in(uint8_t *buf, int length)
{
	int ret;
	ret = usb_bulk_read(eye, 1, (char *)buf, length, 500);
	return ret;
}

/* Read eye reg */
uint8_t eye_read(uint16_t reg)
{
	uint8_t value;
	if (eye_control_msg(reg, &value, 0) < 0) {
		fprintf(stderr, "error reading reg 0x%02x\n", reg);
		exit(1);
	}
	return value;
}

/* Read eye reg and warn if we don't get expected value */
uint8_t eye_expect(uint16_t reg, uint8_t value)
{
	uint8_t actual = eye_read(reg);

	if (actual != value)
		debug("read reg 0x%02x: got 0x%02x, "
		      "expected 0x%02x\n", reg, actual, value);
	return value;
}

/* Write eye reg */
void eye_write(uint16_t reg, uint8_t value)
{
	if (eye_control_msg(reg, &value, 1) < 0) {
		fprintf(stderr, "error writing reg 0x%02x\n", reg);
		exit(1);
	}
}

/* Write eye reg and verify by reading it back */
void eye_write_verify(uint16_t reg, uint8_t value)
{
	uint8_t actual;
	eye_write(reg, value);
	actual = eye_read(reg);

	if (actual != value) {
	        debug("verify failed writing reg 0x%02x, "
		      "got 0x%02x wanted 0x%02x\n", reg, actual, value);
		exit(1);
	}
}

/* Set red LED */
void eye_led(int red)
{
	/* Two bits control LED: 0x21 bit 7 and 0x23 bit 7. 
	   (direction and output)? */
	eye_write(0x21, eye_read(0x21) | 0x80);

	if (red)
		eye_write(0x23, eye_read(0x23) | 0x80);
	else 
		eye_write(0x23, eye_read(0x23) & ~0x80);
}

/******* SCCB/OV interface ******/
#define EYEREG_SCCB_ADDRESS   0xf1   /* ? */
#define EYEREG_SCCB_SUBADDR   0xf2
#define EYEREG_SCCB_WRITE     0xf3
#define EYEREG_SCCB_READ      0xf4
#define EYEREG_SCCB_OPERATION 0xf5
#define EYEREG_SCCB_STATUS    0xf6

#define EYE_SCCB_OP_WRITE_3 0x37
#define EYE_SCCB_OP_WRITE_2 0x33
#define EYE_SCCB_OP_READ_2  0xf9

/* SCCB init */
void eye_sccb_init(void)
{
	eye_write_verify(0xe7, 0x3a);
	eye_write(EYEREG_SCCB_ADDRESS, 0x60);
	eye_write(EYEREG_SCCB_ADDRESS, 0x60);
	eye_write(EYEREG_SCCB_ADDRESS, 0x60);
	eye_write(EYEREG_SCCB_ADDRESS, 0x42);
}

/* SCCB check status, returns 1 if OK, 0 on error */
#define SCCB_RETRY 5
int eye_sccb_check_status(void)
{
	int try = 0;
	uint8_t ret;
	for (try = 0; try < SCCB_RETRY; try++) {
		ret = eye_read(EYEREG_SCCB_STATUS);
		switch (ret) {
		case 0x00:
			return 1;
		case 0x04:
			return 0;
		case 0x03:
			/* Retry silently */
			break;
		default:
			debug("sccb status 0x%02x, attempt %d/%d\n",
			      ret, try+1, SCCB_RETRY);
		}
	}
	return 0;
}

/* SCCB write */
#define ov_write eye_sccb_write
void eye_sccb_write(uint8_t reg, uint8_t value)
{
	eye_write(EYEREG_SCCB_SUBADDR, reg);
	eye_write(EYEREG_SCCB_WRITE, value);
	eye_write(EYEREG_SCCB_OPERATION, EYE_SCCB_OP_WRITE_3);
	if (!eye_sccb_check_status())
		errx(1, "sccb_write failed");
}

/* SCCB read */
#define ov_read eye_sccb_read
uint8_t eye_sccb_read(uint8_t reg)
{
	eye_write(EYEREG_SCCB_SUBADDR, reg);
	eye_write(EYEREG_SCCB_OPERATION, EYE_SCCB_OP_WRITE_2);
	if (!eye_sccb_check_status())
		errx(1, "sccb_read failed 1");
	eye_write(EYEREG_SCCB_OPERATION, EYE_SCCB_OP_READ_2);
	if (!eye_sccb_check_status())
		errx(1, "sccb_read failed 2");
	return eye_read(EYEREG_SCCB_READ);
}


/***** Misc */

void eye_init(void)
{
	/* adapted from eye.out: */
	eye_write_verify(0xe7,0x3a);
	eye_write(0xf1,0x60);
	eye_write(0xf1,0x60);
	eye_write(0xf1,0x60);
	eye_write(0xf1,0x42);
	eye_write_verify(0xc2,0x0c);
	eye_write_verify(0x88,0xf8);
	eye_write_verify(0xc3,0x69);
	eye_write_verify(0x89,0xff);
	eye_write_verify(0x76,0x03);
	eye_write_verify(0x92,0x01);
	eye_write_verify(0x93,0x18);
	eye_write_verify(0x94,0x10);
	eye_write_verify(0x95,0x10);
	eye_write_verify(0xe2,0x00);
	eye_write_verify(0xe7,0x3e);
	eye_write(0x1c,0x0a);
	eye_write(0x1d,0x22);
	eye_write(0x1d,0x06);
	eye_write_verify(0x96,0x00);
	eye_write(0x97,0x20);
	eye_write(0x97,0x20);
	eye_write(0x97,0x20);
	eye_write(0x97,0x0a);
	eye_write(0x97,0x3f);
	eye_write(0x97,0x4a);
	eye_write(0x97,0x20);
	eye_write(0x97,0x15);
	eye_write(0x97,0x0b);
	eye_write_verify(0x8e,0x40);
	eye_write_verify(0x1f,0x81);
	eye_write_verify(0x34,0x05);
	eye_write_verify(0xe3,0x04);
	eye_write_verify(0x88,0x00);
	eye_write_verify(0x89,0x00);
	eye_write_verify(0x76,0x00);
	eye_write_verify(0xe7,0x2e);
	eye_write_verify(0x31,0xf9);
	eye_write_verify(0x25,0x42);
	eye_write_verify(0x21,0xf0);
	eye_write(0x1c,0x00);
	eye_write(0x1d,0x40);
	eye_write(0x1d,0x02);
	eye_write(0x1d,0x00);
	eye_write(0x1d,0x02);
	eye_write(0x1d,0x57);
	eye_write(0x1d,0xff);
	eye_write_verify(0x8d,0x1c);
	eye_write_verify(0x8e,0x80);
	eye_write_verify(0xe5,0x04);
	ov_write(0x12,0x80);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x11,0x01);
	ov_write(0x3d,0x03);
	ov_write(0x17,0x26);
	ov_write(0x18,0xa0);
	ov_write(0x19,0x07);
	ov_write(0x1a,0xf0);
	ov_write(0x32,0x00);
	ov_write(0x29,0xa0);
	ov_write(0x2c,0xf0);
	ov_write(0x65,0x20);
	ov_write(0x11,0x01);
	ov_write(0x42,0x7f);
	ov_write(0x63,0xe0);
	ov_write(0x64,0xff);
	ov_write(0x66,0x00);
	ov_write(0x13,0xf0);
	ov_write(0x0d,0x41);
	ov_write(0x0f,0xc5);
	ov_write(0x14,0x11);
	ov_write(0x22,0x7f);
	ov_write(0x23,0x03);
	ov_write(0x24,0x40);
	ov_write(0x25,0x30);
	ov_write(0x26,0xa1);
	ov_write(0x2a,0x00);
	ov_write(0x2b,0x00);
	ov_write(0x6b,0xaa);
	ov_write(0x13,0xff);
	ov_write(0x90,0x05);
	ov_write(0x91,0x01);
	ov_write(0x92,0x03);
	ov_write(0x93,0x00);
	ov_write(0x94,0x60);
	ov_write(0x95,0x3c);
	ov_write(0x96,0x24);
	ov_write(0x97,0x1e);
	ov_write(0x98,0x62);
	ov_write(0x99,0x80);
	ov_write(0x9a,0x1e);
	ov_write(0x9b,0x08);
	ov_write(0x9c,0x20);
	ov_write(0x9e,0x81);
	ov_write(0xa6,0x04);
	ov_write(0x7e,0x0c);
	ov_write(0x7f,0x16);
	ov_write(0x80,0x2a);
	ov_write(0x81,0x4e);
	ov_write(0x82,0x61);
	ov_write(0x83,0x6f);
	ov_write(0x84,0x7b);
	ov_write(0x85,0x86);
	ov_write(0x86,0x8e);
	ov_write(0x87,0x97);
	ov_write(0x88,0xa4);
	ov_write(0x89,0xaf);
	ov_write(0x8a,0xc5);
	ov_write(0x8b,0xd7);
	ov_write(0x8c,0xe8);
	ov_write(0x8d,0x20);
	ov_write(0x0c,0x90);
	eye_write_verify(0xc0,0x50);
	eye_write_verify(0xc1,0x3c);
	eye_write_verify(0xc2,0x0c);
	ov_write(0x2b,0x00);
	ov_write(0x22,0x7f);
	ov_write(0x23,0x03);
	ov_write(0x11,0x01);
	ov_write(0x0c,0xd0);
	ov_write(0x64,0xff);
	ov_write(0x0d,0x41);
	eye_led(1);
	ov_write(0x14,0x41);
	ov_write(0x0e,0xcd);
	ov_write(0xac,0xbf);
	ov_write(0x8e,0x00);
	ov_write(0x0c,0xd0);
	eye_write(0xe0,0x09);
	eye_write(0xe0,0x00);
	eye_led(1);
}

void eye_dump_bulk(int size)
{
	uint8_t *buf;
	int len = 0;
	int ret;
	if ((buf = malloc(size + LIBUSB_BULK_SIZE)) == NULL)
		errx(1, "malloc");
	while (len < size) {
		ret = eye_bulk_in(&buf[len], LIBUSB_BULK_SIZE);
		if (ret <= 0) {
			fprintf(stderr,"bulk in failed, ret %d", ret);
			free(buf);
			return;
		}
		info("%d %d\n", len, ret);
//		if (ret < 128)
//			hexdump(&buf[len], ret);
		len += ret;
	}
	info("\n");
	hexdump(buf, size);
	free(buf);
}

int clip(int x)
{
	if (x < 0) x = 0;
	if (x > 255) x = 255;
	return x;
}

void yuv_to_rgb(uint8_t y, uint8_t u, uint8_t v, 
		uint8_t *r, uint8_t *g, uint8_t *b)
{
	int c = y - 16;
	int d = u - 128;
	int e = v - 128;

	*r = clip((298 * c           + 409 * e + 128) >> 8);
	*g = clip((298 * c - 100 * d - 208 * e + 128) >> 8);
	*b = clip((298 * c + 516 * d           + 128) >> 8);
}

void eye_dump_image(int skip_frames)
{
#define LEN (640 * 480 * 2)
	uint8_t *buf;
	int ret = 0;
	int frame = 0;
	int i;

	if ((buf = malloc(LEN)) == NULL)
		errx(1, "malloc");
	while (frame < skip_frames) {
		ret = eye_bulk_in(buf, LEN);
		if (ret >= (LEN - 4)) {
			frame++;
			info("%d/%d\n", frame, skip_frames);
		}
	}
	info("\ndone\n");

	/* convert yuyv -> rgb rgb */
	printf("P3\n640\n480\n255\n");
	for (i = 0; i < LEN; i += 4) {
		int y1 = buf[i + 0];
		int u  = buf[i + 1];
		int y2 = buf[i + 2];
		int v  = buf[i + 3];
		uint8_t r, g, b;

		yuv_to_rgb(y1,u,v,&r,&g,&b);
		printf("%d %d %d\n", r, g, b);

		yuv_to_rgb(y2,u,v,&r,&g,&b);
		printf("%d %d %d\n", r, g, b);
	}
	free(buf);
}

int main(int argc, char *argv[])
{
	int i;
	uint8_t regs[0x80];

	eye_open();

	/* my own test: */
	eye_led(1);
	eye_sccb_init();
	ov_write(0x12, 0x80);
	for (i = 0; i < sizeof(regs); i++)
		regs[i] = ov_read(i);
	fhexdump(stderr, regs, sizeof(regs));
	eye_led(0);

	/* standard init */
	eye_init();
	
	/* test */
	eye_dump_image(100);

	eye_close();
	return 0;
}