et61x251_core.c

trident tm5600的linux驱动

free_urbs:
	for (i = 0; (i < ET61X251_URBS) && cam->urb[i]; i++)
		usb_free_urb(cam->urb[i]);

free_buffers:
	for (i = 0; (i < ET61X251_URBS) && cam->transfer_buffer[i]; i++)
		kfree(cam->transfer_buffer[i]);

	return err;
}


static int et61x251_stop_transfer(struct et61x251_device* cam)
{
	struct usb_device *udev = cam->usbdev;
	s8 i;
	int err = 0;

	if (cam->state & DEV_DISCONNECTED)
		return 0;

	for (i = ET61X251_URBS-1; i >= 0; i--) {
		usb_kill_urb(cam->urb[i]);
		usb_free_urb(cam->urb[i]);
		kfree(cam->transfer_buffer[i]);
	}

	err = usb_set_interface(udev, 0, 0); /* 0 Mb/s */
	if (err)
		DBG(3, "usb_set_interface() failed");

	return err;
}


static int et61x251_stream_interrupt(struct et61x251_device* cam)
{
	long timeout;

	cam->stream = STREAM_INTERRUPT;
	timeout = wait_event_timeout(cam->wait_stream,
				     (cam->stream == STREAM_OFF) ||
				     (cam->state & DEV_DISCONNECTED),
				     ET61X251_URB_TIMEOUT);
	if (cam->state & DEV_DISCONNECTED)
		return -ENODEV;
	else if (cam->stream != STREAM_OFF) {
		cam->state |= DEV_MISCONFIGURED;
		DBG(1, "URB timeout reached. The camera is misconfigured. To "
		       "use it, close and open /dev/video%d again.",
		    cam->v4ldev->num);
		return -EIO;
	}

	return 0;
}

/*****************************************************************************/

#ifdef CONFIG_VIDEO_ADV_DEBUG

static int et61x251_i2c_try_read(struct et61x251_device* cam,
				 const struct et61x251_sensor* sensor,
				 u8 address)
{
	struct usb_device* udev = cam->usbdev;
	u8* data = cam->control_buffer;
	int err = 0, res;

	data[0] = address;
	data[1] = cam->sensor.i2c_slave_id;
	data[2] = cam->sensor.rsta | 0x10;
	data[3] = !(et61x251_read_reg(cam, 0x8b) & 0x02);
	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
			      0, 0x88, data, 4, ET61X251_CTRL_TIMEOUT);
	if (res < 0)
		err += res;

	err += et61x251_i2c_wait(cam, sensor);

	res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
			      0, 0x80, data, 8, ET61X251_CTRL_TIMEOUT);
	if (res < 0)
		err += res;

	if (err)
		DBG(3, "I2C read failed for %s image sensor", sensor->name);

	PDBGG("I2C read: address 0x%02X, value: 0x%02X", address, data[0]);

	return err ? -1 : (int)data[0];
}


static int et61x251_i2c_try_write(struct et61x251_device* cam,
				  const struct et61x251_sensor* sensor,
				  u8 address, u8 value)
{
	struct usb_device* udev = cam->usbdev;
	u8* data = cam->control_buffer;
	int err = 0, res;

	data[0] = address;
	data[1] = cam->sensor.i2c_slave_id;
	data[2] = cam->sensor.rsta | 0x12;
	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
			      0, 0x88, data, 3, ET61X251_CTRL_TIMEOUT);
	if (res < 0)
		err += res;

	data[0] = value;
	res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
			      0, 0x80, data, 1, ET61X251_CTRL_TIMEOUT);
	if (res < 0)
		err += res;

	err += et61x251_i2c_wait(cam, sensor);

	if (err)
		DBG(3, "I2C write failed for %s image sensor", sensor->name);

	PDBGG("I2C write: address 0x%02X, value: 0x%02X", address, value);

	return err ? -1 : 0;
}

static int et61x251_i2c_read(struct et61x251_device* cam, u8 address)
{
	return et61x251_i2c_try_read(cam, &cam->sensor, address);
}

static int et61x251_i2c_write(struct et61x251_device* cam,
			      u8 address, u8 value)
{
	return et61x251_i2c_try_write(cam, &cam->sensor, address, value);
}

static u8 et61x251_strtou8(const char* buff, size_t len, ssize_t* count)
{
	char str[5];
	char* endp;
	unsigned long val;

	if (len < 4) {
		strncpy(str, buff, len);
		str[len] = '\0';
	} else {
		strncpy(str, buff, 4);
		str[4] = '\0';
	}

	val = simple_strtoul(str, &endp, 0);

	*count = 0;
	if (val <= 0xff)
		*count = (ssize_t)(endp - str);
	if ((*count) && (len == *count+1) && (buff[*count] == '\n'))
		*count += 1;

	return (u8)val;
}

/*
   NOTE 1: being inside one of the following methods implies that the v4l
	   device exists for sure (see kobjects and reference counters)
   NOTE 2: buffers are PAGE_SIZE long
*/

static ssize_t et61x251_show_reg(struct device* cd,
				 struct device_attribute *attr, char* buf)
{
	struct et61x251_device* cam;
	ssize_t count;

	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
		return -ERESTARTSYS;

	cam = video_get_drvdata(to_video_device(cd));
	if (!cam) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENODEV;
	}

	count = sprintf(buf, "%u\n", cam->sysfs.reg);

	mutex_unlock(&et61x251_sysfs_lock);

	return count;
}


static ssize_t
et61x251_store_reg(struct device* cd,
		   struct device_attribute *attr, const char* buf, size_t len)
{
	struct et61x251_device* cam;
	u8 index;
	ssize_t count;

	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
		return -ERESTARTSYS;

	cam = video_get_drvdata(to_video_device(cd));
	if (!cam) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENODEV;
	}

	index = et61x251_strtou8(buf, len, &count);
	if (index > 0x8e || !count) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -EINVAL;
	}

	cam->sysfs.reg = index;

	DBG(2, "Moved ET61X[12]51 register index to 0x%02X", cam->sysfs.reg);
	DBG(3, "Written bytes: %zd", count);

	mutex_unlock(&et61x251_sysfs_lock);

	return count;
}


static ssize_t et61x251_show_val(struct device* cd,
				 struct device_attribute *attr, char* buf)
{
	struct et61x251_device* cam;
	ssize_t count;
	int val;

	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
		return -ERESTARTSYS;

	cam = video_get_drvdata(to_video_device(cd));
	if (!cam) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENODEV;
	}

	if ((val = et61x251_read_reg(cam, cam->sysfs.reg)) < 0) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -EIO;
	}

	count = sprintf(buf, "%d\n", val);

	DBG(3, "Read bytes: %zd", count);

	mutex_unlock(&et61x251_sysfs_lock);

	return count;
}


static ssize_t
et61x251_store_val(struct device* cd, struct device_attribute *attr,
		   const char* buf, size_t len)
{
	struct et61x251_device* cam;
	u8 value;
	ssize_t count;
	int err;

	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
		return -ERESTARTSYS;

	cam = video_get_drvdata(to_video_device(cd));
	if (!cam) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENODEV;
	}

	value = et61x251_strtou8(buf, len, &count);
	if (!count) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -EINVAL;
	}

	err = et61x251_write_reg(cam, value, cam->sysfs.reg);
	if (err) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -EIO;
	}

	DBG(2, "Written ET61X[12]51 reg. 0x%02X, val. 0x%02X",
	    cam->sysfs.reg, value);
	DBG(3, "Written bytes: %zd", count);

	mutex_unlock(&et61x251_sysfs_lock);

	return count;
}


static ssize_t et61x251_show_i2c_reg(struct device* cd,
				     struct device_attribute *attr, char* buf)
{
	struct et61x251_device* cam;
	ssize_t count;

	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
		return -ERESTARTSYS;

	cam = video_get_drvdata(to_video_device(cd));
	if (!cam) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENODEV;
	}

	count = sprintf(buf, "%u\n", cam->sysfs.i2c_reg);

	DBG(3, "Read bytes: %zd", count);

	mutex_unlock(&et61x251_sysfs_lock);

	return count;
}


static ssize_t
et61x251_store_i2c_reg(struct device* cd, struct device_attribute *attr,
		       const char* buf, size_t len)
{
	struct et61x251_device* cam;
	u8 index;
	ssize_t count;

	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
		return -ERESTARTSYS;

	cam = video_get_drvdata(to_video_device(cd));
	if (!cam) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENODEV;
	}

	index = et61x251_strtou8(buf, len, &count);
	if (!count) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -EINVAL;
	}

	cam->sysfs.i2c_reg = index;

	DBG(2, "Moved sensor register index to 0x%02X", cam->sysfs.i2c_reg);
	DBG(3, "Written bytes: %zd", count);

	mutex_unlock(&et61x251_sysfs_lock);

	return count;
}


static ssize_t et61x251_show_i2c_val(struct device* cd,
				     struct device_attribute *attr, char* buf)
{
	struct et61x251_device* cam;
	ssize_t count;
	int val;

	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
		return -ERESTARTSYS;

	cam = video_get_drvdata(to_video_device(cd));
	if (!cam) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENODEV;
	}

	if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENOSYS;
	}

	if ((val = et61x251_i2c_read(cam, cam->sysfs.i2c_reg)) < 0) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -EIO;
	}

	count = sprintf(buf, "%d\n", val);

	DBG(3, "Read bytes: %zd", count);

	mutex_unlock(&et61x251_sysfs_lock);

	return count;
}


static ssize_t
et61x251_store_i2c_val(struct device* cd, struct device_attribute *attr,
		       const char* buf, size_t len)
{
	struct et61x251_device* cam;
	u8 value;
	ssize_t count;
	int err;

	if (mutex_lock_interruptible(&et61x251_sysfs_lock))
		return -ERESTARTSYS;

	cam = video_get_drvdata(to_video_device(cd));
	if (!cam) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENODEV;
	}

	if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -ENOSYS;
	}

	value = et61x251_strtou8(buf, len, &count);
	if (!count) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -EINVAL;
	}

	err = et61x251_i2c_write(cam, cam->sysfs.i2c_reg, value);
	if (err) {
		mutex_unlock(&et61x251_sysfs_lock);
		return -EIO;
	}

	DBG(2, "Written sensor reg. 0x%02X, val. 0x%02X",
	    cam->sysfs.i2c_reg, value);
	DBG(3, "Written bytes: %zd", count);

	mutex_unlock(&et61x251_sysfs_lock);

	return count;
}


static DEVICE_ATTR(reg, S_IRUGO | S_IWUSR,
		   et61x251_show_reg, et61x251_store_reg);
static DEVICE_ATTR(val, S_IRUGO | S_IWUSR,
		   et61x251_show_val, et61x251_store_val);
static DEVICE_ATTR(i2c_reg, S_IRUGO | S_IWUSR,
		   et61x251_show_i2c_reg, et61x251_store_i2c_reg);
static DEVICE_ATTR(i2c_val, S_IRUGO | S_IWUSR,
		   et61x251_show_i2c_val, et61x251_store_i2c_val);


static int et61x251_create_sysfs(struct et61x251_device* cam)
{
	struct device *classdev = &(cam->v4ldev->dev);
	int err = 0;

	if ((err = device_create_file(classdev, &dev_attr_reg)))
		goto err_out;
	if ((err = device_create_file(classdev, &dev_attr_val)))
		goto err_reg;

	if (cam->sensor.sysfs_ops) {
		if ((err = device_create_file(classdev, &dev_attr_i2c_reg)))
			goto err_val;
		if ((err = device_create_file(classdev, &dev_attr_i2c_val)))
			goto err_i2c_reg;
	}

err_i2c_reg:
	if (cam->sensor.sysfs_ops)
		device_remove_file(classdev, &dev_attr_i2c_reg);
err_val:
	device_remove_file(classdev, &dev_attr_val);
err_reg:
	device_remove_file(classdev, &dev_attr_reg);
err_out:
	return err;
}
#endif /* CONFIG_VIDEO_ADV_DEBUG */

/*****************************************************************************/

static int
et61x251_set_pix_format(struct et61x251_device* cam,
			struct v4l2_pix_format* pix)
{
	int r, err = 0;

	if ((r = et61x251_read_reg(cam, 0x12)) < 0)
		err += r;
	if (pix->pixelformat == V4L2_PIX_FMT_ET61X251)
		err += et61x251_write_reg(cam, r & 0xfd, 0x12);
	else
		err += et61x251_write_reg(cam, r | 0x02, 0x12);

	return err ? -EIO : 0;
}


static int
et61x251_set_compression(struct et61x251_device* cam,
			 struct v4l2_jpegcompression* compression)
{
	int r, err = 0;

	if ((r = et61x251_read_reg(cam, 0x12)) < 0)
		err += r;
	if (compression->quality == 0)
		err += et61x251_write_reg(cam, r & 0xfb, 0x12);
	else
		err += et61x251_write_reg(cam, r | 0x04, 0x12);

	return err ? -EIO : 0;
}


static int et61x251_set_scale(struct et61x251_device* cam, u8 scale)
{
	int r = 0, err = 0;

	r = et61x251_read_reg(cam, 0x12);
	if (r < 0)
		err += r;

	if (scale == 1)
		err += et61x251_write_reg(cam, r & ~0x01, 0x12);
	else if (scale == 2)
		err += et61x251_write_reg(cam, r | 0x01, 0x12);

	if (err)
		return -EIO;

	PDBGG("Scaling factor: %u", scale);

	return 0;
}


static int
et61x251_set_crop(struct et61x251_device* cam, struct v4l2_rect* rect)
{
	struct et61x251_sensor* s = &cam->sensor;
	u16 fmw_sx = (u16)(rect->left - s->cropcap.bounds.left +
			   s->active_pixel.left),
	    fmw_sy = (u16)(rect->top - s->cropcap.bounds.top +
			   s->active_pixel.top),
	    fmw_length = (u16)(rect->width),