ov7670.c

trident tm5600的linux驱动

static int ov7670_cosine(int theta)
{
	theta = 90 - theta;
	if (theta > 180)
		theta -= 360;
	else if (theta < -180)
		theta += 360;
	return ov7670_sine(theta);
}




static void ov7670_calc_cmatrix(struct ov7670_info *info,
		int matrix[CMATRIX_LEN])
{
	int i;
	/*
	 * Apply the current saturation setting first.
	 */
	for (i = 0; i < CMATRIX_LEN; i++)
		matrix[i] = (info->fmt->cmatrix[i]*info->sat) >> 7;
	/*
	 * Then, if need be, rotate the hue value.
	 */
	if (info->hue != 0) {
		int sinth, costh, tmpmatrix[CMATRIX_LEN];

		memcpy(tmpmatrix, matrix, CMATRIX_LEN*sizeof(int));
		sinth = ov7670_sine(info->hue);
		costh = ov7670_cosine(info->hue);

		matrix[0] = (matrix[3]*sinth + matrix[0]*costh)/1000;
		matrix[1] = (matrix[4]*sinth + matrix[1]*costh)/1000;
		matrix[2] = (matrix[5]*sinth + matrix[2]*costh)/1000;
		matrix[3] = (matrix[3]*costh - matrix[0]*sinth)/1000;
		matrix[4] = (matrix[4]*costh - matrix[1]*sinth)/1000;
		matrix[5] = (matrix[5]*costh - matrix[2]*sinth)/1000;
	}
}



static int ov7670_t_sat(struct i2c_client *client, int value)
{
	struct ov7670_info *info = i2c_get_clientdata(client);
	int matrix[CMATRIX_LEN];
	int ret;

	info->sat = value;
	ov7670_calc_cmatrix(info, matrix);
	ret = ov7670_store_cmatrix(client, matrix);
	return ret;
}

static int ov7670_q_sat(struct i2c_client *client, __s32 *value)
{
	struct ov7670_info *info = i2c_get_clientdata(client);

	*value = info->sat;
	return 0;
}

static int ov7670_t_hue(struct i2c_client *client, int value)
{
	struct ov7670_info *info = i2c_get_clientdata(client);
	int matrix[CMATRIX_LEN];
	int ret;

	if (value < -180 || value > 180)
		return -EINVAL;
	info->hue = value;
	ov7670_calc_cmatrix(info, matrix);
	ret = ov7670_store_cmatrix(client, matrix);
	return ret;
}


static int ov7670_q_hue(struct i2c_client *client, __s32 *value)
{
	struct ov7670_info *info = i2c_get_clientdata(client);

	*value = info->hue;
	return 0;
}


/*
 * Some weird registers seem to store values in a sign/magnitude format!
 */
static unsigned char ov7670_sm_to_abs(unsigned char v)
{
	if ((v & 0x80) == 0)
		return v + 128;
	else
		return 128 - (v & 0x7f);
}


static unsigned char ov7670_abs_to_sm(unsigned char v)
{
	if (v > 127)
		return v & 0x7f;
	else
		return (128 - v) | 0x80;
}

static int ov7670_t_brightness(struct i2c_client *client, int value)
{
	unsigned char com8 = 0, v;
	int ret;

	ov7670_read(client, REG_COM8, &com8);
	com8 &= ~COM8_AEC;
	ov7670_write(client, REG_COM8, com8);
	v = ov7670_abs_to_sm(value);
	ret = ov7670_write(client, REG_BRIGHT, v);
	return ret;
}

static int ov7670_q_brightness(struct i2c_client *client, __s32 *value)
{
	unsigned char v = 0;
	int ret = ov7670_read(client, REG_BRIGHT, &v);

	*value = ov7670_sm_to_abs(v);
	return ret;
}

static int ov7670_t_contrast(struct i2c_client *client, int value)
{
	return ov7670_write(client, REG_CONTRAS, (unsigned char) value);
}

static int ov7670_q_contrast(struct i2c_client *client, __s32 *value)
{
	unsigned char v = 0;
	int ret = ov7670_read(client, REG_CONTRAS, &v);

	*value = v;
	return ret;
}

static int ov7670_q_hflip(struct i2c_client *client, __s32 *value)
{
	int ret;
	unsigned char v = 0;

	ret = ov7670_read(client, REG_MVFP, &v);
	*value = (v & MVFP_MIRROR) == MVFP_MIRROR;
	return ret;
}


static int ov7670_t_hflip(struct i2c_client *client, int value)
{
	unsigned char v = 0;
	int ret;

	ret = ov7670_read(client, REG_MVFP, &v);
	if (value)
		v |= MVFP_MIRROR;
	else
		v &= ~MVFP_MIRROR;
	msleep(10);  /* FIXME */
	ret += ov7670_write(client, REG_MVFP, v);
	return ret;
}



static int ov7670_q_vflip(struct i2c_client *client, __s32 *value)
{
	int ret;
	unsigned char v = 0;

	ret = ov7670_read(client, REG_MVFP, &v);
	*value = (v & MVFP_FLIP) == MVFP_FLIP;
	return ret;
}


static int ov7670_t_vflip(struct i2c_client *client, int value)
{
	unsigned char v = 0;
	int ret;

	ret = ov7670_read(client, REG_MVFP, &v);
	if (value)
		v |= MVFP_FLIP;
	else
		v &= ~MVFP_FLIP;
	msleep(10);  /* FIXME */
	ret += ov7670_write(client, REG_MVFP, v);
	return ret;
}


static struct ov7670_control {
	struct v4l2_queryctrl qc;
	int (*query)(struct i2c_client *c, __s32 *value);
	int (*tweak)(struct i2c_client *c, int value);
} ov7670_controls[] =
{
	{
		.qc = {
			.id = V4L2_CID_BRIGHTNESS,
			.type = V4L2_CTRL_TYPE_INTEGER,
			.name = "Brightness",
			.minimum = 0,
			.maximum = 255,
			.step = 1,
			.default_value = 0x80,
			.flags = V4L2_CTRL_FLAG_SLIDER
		},
		.tweak = ov7670_t_brightness,
		.query = ov7670_q_brightness,
	},
	{
		.qc = {
			.id = V4L2_CID_CONTRAST,
			.type = V4L2_CTRL_TYPE_INTEGER,
			.name = "Contrast",
			.minimum = 0,
			.maximum = 127,
			.step = 1,
			.default_value = 0x40,   /* XXX ov7670 spec */
			.flags = V4L2_CTRL_FLAG_SLIDER
		},
		.tweak = ov7670_t_contrast,
		.query = ov7670_q_contrast,
	},
	{
		.qc = {
			.id = V4L2_CID_SATURATION,
			.type = V4L2_CTRL_TYPE_INTEGER,
			.name = "Saturation",
			.minimum = 0,
			.maximum = 256,
			.step = 1,
			.default_value = 0x80,
			.flags = V4L2_CTRL_FLAG_SLIDER
		},
		.tweak = ov7670_t_sat,
		.query = ov7670_q_sat,
	},
	{
		.qc = {
			.id = V4L2_CID_HUE,
			.type = V4L2_CTRL_TYPE_INTEGER,
			.name = "HUE",
			.minimum = -180,
			.maximum = 180,
			.step = 5,
			.default_value = 0,
			.flags = V4L2_CTRL_FLAG_SLIDER
		},
		.tweak = ov7670_t_hue,
		.query = ov7670_q_hue,
	},
	{
		.qc = {
			.id = V4L2_CID_VFLIP,
			.type = V4L2_CTRL_TYPE_BOOLEAN,
			.name = "Vertical flip",
			.minimum = 0,
			.maximum = 1,
			.step = 1,
			.default_value = 0,
		},
		.tweak = ov7670_t_vflip,
		.query = ov7670_q_vflip,
	},
	{
		.qc = {
			.id = V4L2_CID_HFLIP,
			.type = V4L2_CTRL_TYPE_BOOLEAN,
			.name = "Horizontal mirror",
			.minimum = 0,
			.maximum = 1,
			.step = 1,
			.default_value = 0,
		},
		.tweak = ov7670_t_hflip,
		.query = ov7670_q_hflip,
	},
};
#define N_CONTROLS (ARRAY_SIZE(ov7670_controls))

static struct ov7670_control *ov7670_find_control(__u32 id)
{
	int i;

	for (i = 0; i < N_CONTROLS; i++)
		if (ov7670_controls[i].qc.id == id)
			return ov7670_controls + i;
	return NULL;
}


static int ov7670_queryctrl(struct i2c_client *client,
		struct v4l2_queryctrl *qc)
{
	struct ov7670_control *ctrl = ov7670_find_control(qc->id);

	if (ctrl == NULL)
		return -EINVAL;
	*qc = ctrl->qc;
	return 0;
}

static int ov7670_g_ctrl(struct i2c_client *client, struct v4l2_control *ctrl)
{
	struct ov7670_control *octrl = ov7670_find_control(ctrl->id);
	int ret;

	if (octrl == NULL)
		return -EINVAL;
	ret = octrl->query(client, &ctrl->value);
	if (ret >= 0)
		return 0;
	return ret;
}

static int ov7670_s_ctrl(struct i2c_client *client, struct v4l2_control *ctrl)
{
	struct ov7670_control *octrl = ov7670_find_control(ctrl->id);
	int ret;

	if (octrl == NULL)
		return -EINVAL;
	ret =  octrl->tweak(client, ctrl->value);
	if (ret >= 0)
		return 0;
	return ret;
}






/*
 * Basic i2c stuff.
 */
static struct i2c_driver ov7670_driver;

static int ov7670_attach(struct i2c_adapter *adapter)
{
	int ret;
	struct i2c_client *client;
	struct ov7670_info *info;

	/*
	 * For now: only deal with adapters we recognize.
	 */
	if (adapter->id != I2C_HW_SMBUS_CAFE)
		return -ENODEV;

	client = kzalloc(sizeof (struct i2c_client), GFP_KERNEL);
	if (! client)
		return -ENOMEM;
	client->adapter = adapter;
	client->addr = OV7670_I2C_ADDR;
	client->driver = &ov7670_driver,
	strcpy(client->name, "OV7670");
	/*
	 * Set up our info structure.
	 */
	info = kzalloc(sizeof (struct ov7670_info), GFP_KERNEL);
	if (! info) {
		ret = -ENOMEM;
		goto out_free;
	}
	info->fmt = &ov7670_formats[0];
	info->sat = 128;	/* Review this */
	i2c_set_clientdata(client, info);

	/*
	 * Make sure it's an ov7670
	 */
	ret = ov7670_detect(client);
	if (ret)
		goto out_free_info;
	ret = i2c_attach_client(client);
	if (ret)
		goto out_free_info;
	return 0;

  out_free_info:
	kfree(info);
  out_free:
	kfree(client);
	return ret;
}


static int ov7670_detach(struct i2c_client *client)
{
	i2c_detach_client(client);
	kfree(i2c_get_clientdata(client));
	kfree(client);
	return 0;
}


static int ov7670_command(struct i2c_client *client, unsigned int cmd,
		void *arg)
{
	switch (cmd) {
	case VIDIOC_G_CHIP_IDENT:
		return v4l2_chip_ident_i2c_client(client, arg, V4L2_IDENT_OV7670, 0);

	case VIDIOC_INT_RESET:
		ov7670_reset(client);
		return 0;

	case VIDIOC_INT_INIT:
		return ov7670_init(client);

	case VIDIOC_ENUM_FMT:
		return ov7670_enum_fmt(client, (struct v4l2_fmtdesc *) arg);
	case VIDIOC_TRY_FMT:
		return ov7670_try_fmt(client, (struct v4l2_format *) arg, NULL, NULL);
	case VIDIOC_S_FMT:
		return ov7670_s_fmt(client, (struct v4l2_format *) arg);
	case VIDIOC_QUERYCTRL:
		return ov7670_queryctrl(client, (struct v4l2_queryctrl *) arg);
	case VIDIOC_S_CTRL:
		return ov7670_s_ctrl(client, (struct v4l2_control *) arg);
	case VIDIOC_G_CTRL:
		return ov7670_g_ctrl(client, (struct v4l2_control *) arg);
	case VIDIOC_S_PARM:
		return ov7670_s_parm(client, (struct v4l2_streamparm *) arg);
	case VIDIOC_G_PARM:
		return ov7670_g_parm(client, (struct v4l2_streamparm *) arg);
	}
	return -EINVAL;
}



static struct i2c_driver ov7670_driver = {
	.driver = {
		.name = "ov7670",
	},
	.id 		= I2C_DRIVERID_OV7670,
	.class 		= I2C_CLASS_CAM_DIGITAL,
	.attach_adapter = ov7670_attach,
	.detach_client	= ov7670_detach,
	.command	= ov7670_command,
};


/*
 * Module initialization
 */
static int __init ov7670_mod_init(void)
{
	printk(KERN_NOTICE "OmniVision ov7670 sensor driver, at your service\n");
	return i2c_add_driver(&ov7670_driver);
}

static void __exit ov7670_mod_exit(void)
{
	i2c_del_driver(&ov7670_driver);
}

module_init(ov7670_mod_init);
module_exit(ov7670_mod_exit);