PageRenderTime 103ms CodeModel.GetById 41ms app.highlight 56ms RepoModel.GetById 1ms app.codeStats 0ms

/drivers/media/video/mt9t031.c

https://bitbucket.org/ndreys/linux-sunxi
C | 934 lines | 702 code | 151 blank | 81 comment | 81 complexity | 58427ad11c130a19bccb4808d137a7c6 MD5 | raw file
Possible License(s): GPL-2.0, LGPL-2.0, AGPL-1.0
  1/*
  2 * Driver for MT9T031 CMOS Image Sensor from Micron
  3 *
  4 * Copyright (C) 2008, Guennadi Liakhovetski, DENX Software Engineering <lg@denx.de>
  5 *
  6 * This program is free software; you can redistribute it and/or modify
  7 * it under the terms of the GNU General Public License version 2 as
  8 * published by the Free Software Foundation.
  9 */
 10
 11#include <linux/device.h>
 12#include <linux/i2c.h>
 13#include <linux/log2.h>
 14#include <linux/pm.h>
 15#include <linux/slab.h>
 16#include <linux/videodev2.h>
 17
 18#include <media/soc_camera.h>
 19#include <media/v4l2-chip-ident.h>
 20#include <media/v4l2-subdev.h>
 21
 22/*
 23 * mt9t031 i2c address 0x5d
 24 * The platform has to define i2c_board_info and link to it from
 25 * struct soc_camera_link
 26 */
 27
 28/* mt9t031 selected register addresses */
 29#define MT9T031_CHIP_VERSION		0x00
 30#define MT9T031_ROW_START		0x01
 31#define MT9T031_COLUMN_START		0x02
 32#define MT9T031_WINDOW_HEIGHT		0x03
 33#define MT9T031_WINDOW_WIDTH		0x04
 34#define MT9T031_HORIZONTAL_BLANKING	0x05
 35#define MT9T031_VERTICAL_BLANKING	0x06
 36#define MT9T031_OUTPUT_CONTROL		0x07
 37#define MT9T031_SHUTTER_WIDTH_UPPER	0x08
 38#define MT9T031_SHUTTER_WIDTH		0x09
 39#define MT9T031_PIXEL_CLOCK_CONTROL	0x0a
 40#define MT9T031_FRAME_RESTART		0x0b
 41#define MT9T031_SHUTTER_DELAY		0x0c
 42#define MT9T031_RESET			0x0d
 43#define MT9T031_READ_MODE_1		0x1e
 44#define MT9T031_READ_MODE_2		0x20
 45#define MT9T031_READ_MODE_3		0x21
 46#define MT9T031_ROW_ADDRESS_MODE	0x22
 47#define MT9T031_COLUMN_ADDRESS_MODE	0x23
 48#define MT9T031_GLOBAL_GAIN		0x35
 49#define MT9T031_CHIP_ENABLE		0xF8
 50
 51#define MT9T031_MAX_HEIGHT		1536
 52#define MT9T031_MAX_WIDTH		2048
 53#define MT9T031_MIN_HEIGHT		2
 54#define MT9T031_MIN_WIDTH		18
 55#define MT9T031_HORIZONTAL_BLANK	142
 56#define MT9T031_VERTICAL_BLANK		25
 57#define MT9T031_COLUMN_SKIP		32
 58#define MT9T031_ROW_SKIP		20
 59
 60#define MT9T031_BUS_PARAM	(SOCAM_PCLK_SAMPLE_RISING |	\
 61	SOCAM_PCLK_SAMPLE_FALLING | SOCAM_HSYNC_ACTIVE_HIGH |	\
 62	SOCAM_VSYNC_ACTIVE_HIGH | SOCAM_DATA_ACTIVE_HIGH |	\
 63	SOCAM_MASTER | SOCAM_DATAWIDTH_10)
 64
 65struct mt9t031 {
 66	struct v4l2_subdev subdev;
 67	struct v4l2_rect rect;	/* Sensor window */
 68	int model;	/* V4L2_IDENT_MT9T031* codes from v4l2-chip-ident.h */
 69	u16 xskip;
 70	u16 yskip;
 71	unsigned int gain;
 72	unsigned short y_skip_top;	/* Lines to skip at the top */
 73	unsigned int exposure;
 74	unsigned char autoexposure;
 75};
 76
 77static struct mt9t031 *to_mt9t031(const struct i2c_client *client)
 78{
 79	return container_of(i2c_get_clientdata(client), struct mt9t031, subdev);
 80}
 81
 82static int reg_read(struct i2c_client *client, const u8 reg)
 83{
 84	s32 data = i2c_smbus_read_word_data(client, reg);
 85	return data < 0 ? data : swab16(data);
 86}
 87
 88static int reg_write(struct i2c_client *client, const u8 reg,
 89		     const u16 data)
 90{
 91	return i2c_smbus_write_word_data(client, reg, swab16(data));
 92}
 93
 94static int reg_set(struct i2c_client *client, const u8 reg,
 95		   const u16 data)
 96{
 97	int ret;
 98
 99	ret = reg_read(client, reg);
100	if (ret < 0)
101		return ret;
102	return reg_write(client, reg, ret | data);
103}
104
105static int reg_clear(struct i2c_client *client, const u8 reg,
106		     const u16 data)
107{
108	int ret;
109
110	ret = reg_read(client, reg);
111	if (ret < 0)
112		return ret;
113	return reg_write(client, reg, ret & ~data);
114}
115
116static int set_shutter(struct i2c_client *client, const u32 data)
117{
118	int ret;
119
120	ret = reg_write(client, MT9T031_SHUTTER_WIDTH_UPPER, data >> 16);
121
122	if (ret >= 0)
123		ret = reg_write(client, MT9T031_SHUTTER_WIDTH, data & 0xffff);
124
125	return ret;
126}
127
128static int get_shutter(struct i2c_client *client, u32 *data)
129{
130	int ret;
131
132	ret = reg_read(client, MT9T031_SHUTTER_WIDTH_UPPER);
133	*data = ret << 16;
134
135	if (ret >= 0)
136		ret = reg_read(client, MT9T031_SHUTTER_WIDTH);
137	*data |= ret & 0xffff;
138
139	return ret < 0 ? ret : 0;
140}
141
142static int mt9t031_idle(struct i2c_client *client)
143{
144	int ret;
145
146	/* Disable chip output, synchronous option update */
147	ret = reg_write(client, MT9T031_RESET, 1);
148	if (ret >= 0)
149		ret = reg_write(client, MT9T031_RESET, 0);
150	if (ret >= 0)
151		ret = reg_clear(client, MT9T031_OUTPUT_CONTROL, 2);
152
153	return ret >= 0 ? 0 : -EIO;
154}
155
156static int mt9t031_disable(struct i2c_client *client)
157{
158	/* Disable the chip */
159	reg_clear(client, MT9T031_OUTPUT_CONTROL, 2);
160
161	return 0;
162}
163
164static int mt9t031_s_stream(struct v4l2_subdev *sd, int enable)
165{
166	struct i2c_client *client = v4l2_get_subdevdata(sd);
167	int ret;
168
169	if (enable)
170		/* Switch to master "normal" mode */
171		ret = reg_set(client, MT9T031_OUTPUT_CONTROL, 2);
172	else
173		/* Stop sensor readout */
174		ret = reg_clear(client, MT9T031_OUTPUT_CONTROL, 2);
175
176	if (ret < 0)
177		return -EIO;
178
179	return 0;
180}
181
182static int mt9t031_set_bus_param(struct soc_camera_device *icd,
183				 unsigned long flags)
184{
185	struct i2c_client *client = to_i2c_client(to_soc_camera_control(icd));
186
187	/* The caller should have queried our parameters, check anyway */
188	if (flags & ~MT9T031_BUS_PARAM)
189		return -EINVAL;
190
191	if (flags & SOCAM_PCLK_SAMPLE_FALLING)
192		reg_clear(client, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
193	else
194		reg_set(client, MT9T031_PIXEL_CLOCK_CONTROL, 0x8000);
195
196	return 0;
197}
198
199static unsigned long mt9t031_query_bus_param(struct soc_camera_device *icd)
200{
201	struct soc_camera_link *icl = to_soc_camera_link(icd);
202
203	return soc_camera_apply_sensor_flags(icl, MT9T031_BUS_PARAM);
204}
205
206enum {
207	MT9T031_CTRL_VFLIP,
208	MT9T031_CTRL_HFLIP,
209	MT9T031_CTRL_GAIN,
210	MT9T031_CTRL_EXPOSURE,
211	MT9T031_CTRL_EXPOSURE_AUTO,
212};
213
214static const struct v4l2_queryctrl mt9t031_controls[] = {
215	[MT9T031_CTRL_VFLIP] = {
216		.id		= V4L2_CID_VFLIP,
217		.type		= V4L2_CTRL_TYPE_BOOLEAN,
218		.name		= "Flip Vertically",
219		.minimum	= 0,
220		.maximum	= 1,
221		.step		= 1,
222		.default_value	= 0,
223	},
224	[MT9T031_CTRL_HFLIP] = {
225		.id		= V4L2_CID_HFLIP,
226		.type		= V4L2_CTRL_TYPE_BOOLEAN,
227		.name		= "Flip Horizontally",
228		.minimum	= 0,
229		.maximum	= 1,
230		.step		= 1,
231		.default_value	= 0,
232	},
233	[MT9T031_CTRL_GAIN] = {
234		.id		= V4L2_CID_GAIN,
235		.type		= V4L2_CTRL_TYPE_INTEGER,
236		.name		= "Gain",
237		.minimum	= 0,
238		.maximum	= 127,
239		.step		= 1,
240		.default_value	= 64,
241		.flags		= V4L2_CTRL_FLAG_SLIDER,
242	},
243	[MT9T031_CTRL_EXPOSURE] = {
244		.id		= V4L2_CID_EXPOSURE,
245		.type		= V4L2_CTRL_TYPE_INTEGER,
246		.name		= "Exposure",
247		.minimum	= 1,
248		.maximum	= 255,
249		.step		= 1,
250		.default_value	= 255,
251		.flags		= V4L2_CTRL_FLAG_SLIDER,
252	},
253	[MT9T031_CTRL_EXPOSURE_AUTO] = {
254		.id		= V4L2_CID_EXPOSURE_AUTO,
255		.type		= V4L2_CTRL_TYPE_BOOLEAN,
256		.name		= "Automatic Exposure",
257		.minimum	= 0,
258		.maximum	= 1,
259		.step		= 1,
260		.default_value	= 1,
261	}
262};
263
264static struct soc_camera_ops mt9t031_ops = {
265	.set_bus_param		= mt9t031_set_bus_param,
266	.query_bus_param	= mt9t031_query_bus_param,
267	.controls		= mt9t031_controls,
268	.num_controls		= ARRAY_SIZE(mt9t031_controls),
269};
270
271/* target must be _even_ */
272static u16 mt9t031_skip(s32 *source, s32 target, s32 max)
273{
274	unsigned int skip;
275
276	if (*source < target + target / 2) {
277		*source = target;
278		return 1;
279	}
280
281	skip = min(max, *source + target / 2) / target;
282	if (skip > 8)
283		skip = 8;
284	*source = target * skip;
285
286	return skip;
287}
288
289/* rect is the sensor rectangle, the caller guarantees parameter validity */
290static int mt9t031_set_params(struct i2c_client *client,
291			      struct v4l2_rect *rect, u16 xskip, u16 yskip)
292{
293	struct mt9t031 *mt9t031 = to_mt9t031(client);
294	int ret;
295	u16 xbin, ybin;
296	const u16 hblank = MT9T031_HORIZONTAL_BLANK,
297		vblank = MT9T031_VERTICAL_BLANK;
298
299	xbin = min(xskip, (u16)3);
300	ybin = min(yskip, (u16)3);
301
302	/*
303	 * Could just do roundup(rect->left, [xy]bin * 2); but this is cheaper.
304	 * There is always a valid suitably aligned value. The worst case is
305	 * xbin = 3, width = 2048. Then we will start at 36, the last read out
306	 * pixel will be 2083, which is < 2085 - first black pixel.
307	 *
308	 * MT9T031 datasheet imposes window left border alignment, depending on
309	 * the selected xskip. Failing to conform to this requirement produces
310	 * dark horizontal stripes in the image. However, even obeying to this
311	 * requirement doesn't eliminate the stripes in all configurations. They
312	 * appear "locally reproducibly," but can differ between tests under
313	 * different lighting conditions.
314	 */
315	switch (xbin) {
316	case 1:
317		rect->left &= ~1;
318		break;
319	case 2:
320		rect->left &= ~3;
321		break;
322	case 3:
323		rect->left = rect->left > roundup(MT9T031_COLUMN_SKIP, 6) ?
324			(rect->left / 6) * 6 : roundup(MT9T031_COLUMN_SKIP, 6);
325	}
326
327	rect->top &= ~1;
328
329	dev_dbg(&client->dev, "skip %u:%u, rect %ux%u@%u:%u\n",
330		xskip, yskip, rect->width, rect->height, rect->left, rect->top);
331
332	/* Disable register update, reconfigure atomically */
333	ret = reg_set(client, MT9T031_OUTPUT_CONTROL, 1);
334	if (ret < 0)
335		return ret;
336
337	/* Blanking and start values - default... */
338	ret = reg_write(client, MT9T031_HORIZONTAL_BLANKING, hblank);
339	if (ret >= 0)
340		ret = reg_write(client, MT9T031_VERTICAL_BLANKING, vblank);
341
342	if (yskip != mt9t031->yskip || xskip != mt9t031->xskip) {
343		/* Binning, skipping */
344		if (ret >= 0)
345			ret = reg_write(client, MT9T031_COLUMN_ADDRESS_MODE,
346					((xbin - 1) << 4) | (xskip - 1));
347		if (ret >= 0)
348			ret = reg_write(client, MT9T031_ROW_ADDRESS_MODE,
349					((ybin - 1) << 4) | (yskip - 1));
350	}
351	dev_dbg(&client->dev, "new physical left %u, top %u\n",
352		rect->left, rect->top);
353
354	/*
355	 * The caller provides a supported format, as guaranteed by
356	 * icd->try_fmt_cap(), soc_camera_s_crop() and soc_camera_cropcap()
357	 */
358	if (ret >= 0)
359		ret = reg_write(client, MT9T031_COLUMN_START, rect->left);
360	if (ret >= 0)
361		ret = reg_write(client, MT9T031_ROW_START, rect->top);
362	if (ret >= 0)
363		ret = reg_write(client, MT9T031_WINDOW_WIDTH, rect->width - 1);
364	if (ret >= 0)
365		ret = reg_write(client, MT9T031_WINDOW_HEIGHT,
366				rect->height + mt9t031->y_skip_top - 1);
367	if (ret >= 0 && mt9t031->autoexposure) {
368		unsigned int total_h = rect->height + mt9t031->y_skip_top + vblank;
369		ret = set_shutter(client, total_h);
370		if (ret >= 0) {
371			const u32 shutter_max = MT9T031_MAX_HEIGHT + vblank;
372			const struct v4l2_queryctrl *qctrl =
373				&mt9t031_controls[MT9T031_CTRL_EXPOSURE];
374			mt9t031->exposure = (shutter_max / 2 + (total_h - 1) *
375				 (qctrl->maximum - qctrl->minimum)) /
376				shutter_max + qctrl->minimum;
377		}
378	}
379
380	/* Re-enable register update, commit all changes */
381	if (ret >= 0)
382		ret = reg_clear(client, MT9T031_OUTPUT_CONTROL, 1);
383
384	if (ret >= 0) {
385		mt9t031->rect = *rect;
386		mt9t031->xskip = xskip;
387		mt9t031->yskip = yskip;
388	}
389
390	return ret < 0 ? ret : 0;
391}
392
393static int mt9t031_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
394{
395	struct v4l2_rect rect = a->c;
396	struct i2c_client *client = v4l2_get_subdevdata(sd);
397	struct mt9t031 *mt9t031 = to_mt9t031(client);
398
399	rect.width = ALIGN(rect.width, 2);
400	rect.height = ALIGN(rect.height, 2);
401
402	soc_camera_limit_side(&rect.left, &rect.width,
403		     MT9T031_COLUMN_SKIP, MT9T031_MIN_WIDTH, MT9T031_MAX_WIDTH);
404
405	soc_camera_limit_side(&rect.top, &rect.height,
406		     MT9T031_ROW_SKIP, MT9T031_MIN_HEIGHT, MT9T031_MAX_HEIGHT);
407
408	return mt9t031_set_params(client, &rect, mt9t031->xskip, mt9t031->yskip);
409}
410
411static int mt9t031_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
412{
413	struct i2c_client *client = v4l2_get_subdevdata(sd);
414	struct mt9t031 *mt9t031 = to_mt9t031(client);
415
416	a->c	= mt9t031->rect;
417	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
418
419	return 0;
420}
421
422static int mt9t031_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
423{
424	a->bounds.left			= MT9T031_COLUMN_SKIP;
425	a->bounds.top			= MT9T031_ROW_SKIP;
426	a->bounds.width			= MT9T031_MAX_WIDTH;
427	a->bounds.height		= MT9T031_MAX_HEIGHT;
428	a->defrect			= a->bounds;
429	a->type				= V4L2_BUF_TYPE_VIDEO_CAPTURE;
430	a->pixelaspect.numerator	= 1;
431	a->pixelaspect.denominator	= 1;
432
433	return 0;
434}
435
436static int mt9t031_g_fmt(struct v4l2_subdev *sd,
437			 struct v4l2_mbus_framefmt *mf)
438{
439	struct i2c_client *client = v4l2_get_subdevdata(sd);
440	struct mt9t031 *mt9t031 = to_mt9t031(client);
441
442	mf->width	= mt9t031->rect.width / mt9t031->xskip;
443	mf->height	= mt9t031->rect.height / mt9t031->yskip;
444	mf->code	= V4L2_MBUS_FMT_SBGGR10_1X10;
445	mf->colorspace	= V4L2_COLORSPACE_SRGB;
446	mf->field	= V4L2_FIELD_NONE;
447
448	return 0;
449}
450
451static int mt9t031_s_fmt(struct v4l2_subdev *sd,
452			 struct v4l2_mbus_framefmt *mf)
453{
454	struct i2c_client *client = v4l2_get_subdevdata(sd);
455	struct mt9t031 *mt9t031 = to_mt9t031(client);
456	u16 xskip, yskip;
457	struct v4l2_rect rect = mt9t031->rect;
458
459	/*
460	 * try_fmt has put width and height within limits.
461	 * S_FMT: use binning and skipping for scaling
462	 */
463	xskip = mt9t031_skip(&rect.width, mf->width, MT9T031_MAX_WIDTH);
464	yskip = mt9t031_skip(&rect.height, mf->height, MT9T031_MAX_HEIGHT);
465
466	mf->code	= V4L2_MBUS_FMT_SBGGR10_1X10;
467	mf->colorspace	= V4L2_COLORSPACE_SRGB;
468
469	/* mt9t031_set_params() doesn't change width and height */
470	return mt9t031_set_params(client, &rect, xskip, yskip);
471}
472
473/*
474 * If a user window larger than sensor window is requested, we'll increase the
475 * sensor window.
476 */
477static int mt9t031_try_fmt(struct v4l2_subdev *sd,
478			   struct v4l2_mbus_framefmt *mf)
479{
480	v4l_bound_align_image(
481		&mf->width, MT9T031_MIN_WIDTH, MT9T031_MAX_WIDTH, 1,
482		&mf->height, MT9T031_MIN_HEIGHT, MT9T031_MAX_HEIGHT, 1, 0);
483
484	mf->code	= V4L2_MBUS_FMT_SBGGR10_1X10;
485	mf->colorspace	= V4L2_COLORSPACE_SRGB;
486
487	return 0;
488}
489
490static int mt9t031_g_chip_ident(struct v4l2_subdev *sd,
491				struct v4l2_dbg_chip_ident *id)
492{
493	struct i2c_client *client = v4l2_get_subdevdata(sd);
494	struct mt9t031 *mt9t031 = to_mt9t031(client);
495
496	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
497		return -EINVAL;
498
499	if (id->match.addr != client->addr)
500		return -ENODEV;
501
502	id->ident	= mt9t031->model;
503	id->revision	= 0;
504
505	return 0;
506}
507
508#ifdef CONFIG_VIDEO_ADV_DEBUG
509static int mt9t031_g_register(struct v4l2_subdev *sd,
510			      struct v4l2_dbg_register *reg)
511{
512	struct i2c_client *client = v4l2_get_subdevdata(sd);
513
514	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
515		return -EINVAL;
516
517	if (reg->match.addr != client->addr)
518		return -ENODEV;
519
520	reg->val = reg_read(client, reg->reg);
521
522	if (reg->val > 0xffff)
523		return -EIO;
524
525	return 0;
526}
527
528static int mt9t031_s_register(struct v4l2_subdev *sd,
529			      struct v4l2_dbg_register *reg)
530{
531	struct i2c_client *client = v4l2_get_subdevdata(sd);
532
533	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0xff)
534		return -EINVAL;
535
536	if (reg->match.addr != client->addr)
537		return -ENODEV;
538
539	if (reg_write(client, reg->reg, reg->val) < 0)
540		return -EIO;
541
542	return 0;
543}
544#endif
545
546static int mt9t031_g_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
547{
548	struct i2c_client *client = v4l2_get_subdevdata(sd);
549	struct mt9t031 *mt9t031 = to_mt9t031(client);
550	int data;
551
552	switch (ctrl->id) {
553	case V4L2_CID_VFLIP:
554		data = reg_read(client, MT9T031_READ_MODE_2);
555		if (data < 0)
556			return -EIO;
557		ctrl->value = !!(data & 0x8000);
558		break;
559	case V4L2_CID_HFLIP:
560		data = reg_read(client, MT9T031_READ_MODE_2);
561		if (data < 0)
562			return -EIO;
563		ctrl->value = !!(data & 0x4000);
564		break;
565	case V4L2_CID_EXPOSURE_AUTO:
566		ctrl->value = mt9t031->autoexposure;
567		break;
568	case V4L2_CID_GAIN:
569		ctrl->value = mt9t031->gain;
570		break;
571	case V4L2_CID_EXPOSURE:
572		ctrl->value = mt9t031->exposure;
573		break;
574	}
575	return 0;
576}
577
578static int mt9t031_s_ctrl(struct v4l2_subdev *sd, struct v4l2_control *ctrl)
579{
580	struct i2c_client *client = v4l2_get_subdevdata(sd);
581	struct mt9t031 *mt9t031 = to_mt9t031(client);
582	const struct v4l2_queryctrl *qctrl;
583	int data;
584
585	switch (ctrl->id) {
586	case V4L2_CID_VFLIP:
587		if (ctrl->value)
588			data = reg_set(client, MT9T031_READ_MODE_2, 0x8000);
589		else
590			data = reg_clear(client, MT9T031_READ_MODE_2, 0x8000);
591		if (data < 0)
592			return -EIO;
593		break;
594	case V4L2_CID_HFLIP:
595		if (ctrl->value)
596			data = reg_set(client, MT9T031_READ_MODE_2, 0x4000);
597		else
598			data = reg_clear(client, MT9T031_READ_MODE_2, 0x4000);
599		if (data < 0)
600			return -EIO;
601		break;
602	case V4L2_CID_GAIN:
603		qctrl = &mt9t031_controls[MT9T031_CTRL_GAIN];
604		if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
605			return -EINVAL;
606		/* See Datasheet Table 7, Gain settings. */
607		if (ctrl->value <= qctrl->default_value) {
608			/* Pack it into 0..1 step 0.125, register values 0..8 */
609			unsigned long range = qctrl->default_value - qctrl->minimum;
610			data = ((ctrl->value - qctrl->minimum) * 8 + range / 2) / range;
611
612			dev_dbg(&client->dev, "Setting gain %d\n", data);
613			data = reg_write(client, MT9T031_GLOBAL_GAIN, data);
614			if (data < 0)
615				return -EIO;
616		} else {
617			/* Pack it into 1.125..128 variable step, register values 9..0x7860 */
618			/* We assume qctrl->maximum - qctrl->default_value - 1 > 0 */
619			unsigned long range = qctrl->maximum - qctrl->default_value - 1;
620			/* calculated gain: map 65..127 to 9..1024 step 0.125 */
621			unsigned long gain = ((ctrl->value - qctrl->default_value - 1) *
622					       1015 + range / 2) / range + 9;
623
624			if (gain <= 32)		/* calculated gain 9..32 -> 9..32 */
625				data = gain;
626			else if (gain <= 64)	/* calculated gain 33..64 -> 0x51..0x60 */
627				data = ((gain - 32) * 16 + 16) / 32 + 80;
628			else
629				/* calculated gain 65..1024 -> (1..120) << 8 + 0x60 */
630				data = (((gain - 64 + 7) * 32) & 0xff00) | 0x60;
631
632			dev_dbg(&client->dev, "Set gain from 0x%x to 0x%x\n",
633				reg_read(client, MT9T031_GLOBAL_GAIN), data);
634			data = reg_write(client, MT9T031_GLOBAL_GAIN, data);
635			if (data < 0)
636				return -EIO;
637		}
638
639		/* Success */
640		mt9t031->gain = ctrl->value;
641		break;
642	case V4L2_CID_EXPOSURE:
643		qctrl = &mt9t031_controls[MT9T031_CTRL_EXPOSURE];
644		/* mt9t031 has maximum == default */
645		if (ctrl->value > qctrl->maximum || ctrl->value < qctrl->minimum)
646			return -EINVAL;
647		else {
648			const unsigned long range = qctrl->maximum - qctrl->minimum;
649			const u32 shutter = ((ctrl->value - qctrl->minimum) * 1048 +
650					     range / 2) / range + 1;
651			u32 old;
652
653			get_shutter(client, &old);
654			dev_dbg(&client->dev, "Set shutter from %u to %u\n",
655				old, shutter);
656			if (set_shutter(client, shutter) < 0)
657				return -EIO;
658			mt9t031->exposure = ctrl->value;
659			mt9t031->autoexposure = 0;
660		}
661		break;
662	case V4L2_CID_EXPOSURE_AUTO:
663		if (ctrl->value) {
664			const u16 vblank = MT9T031_VERTICAL_BLANK;
665			const u32 shutter_max = MT9T031_MAX_HEIGHT + vblank;
666			unsigned int total_h = mt9t031->rect.height +
667				mt9t031->y_skip_top + vblank;
668
669			if (set_shutter(client, total_h) < 0)
670				return -EIO;
671			qctrl = &mt9t031_controls[MT9T031_CTRL_EXPOSURE];
672			mt9t031->exposure = (shutter_max / 2 + (total_h - 1) *
673				 (qctrl->maximum - qctrl->minimum)) /
674				shutter_max + qctrl->minimum;
675			mt9t031->autoexposure = 1;
676		} else
677			mt9t031->autoexposure = 0;
678		break;
679	default:
680		return -EINVAL;
681	}
682	return 0;
683}
684
685/*
686 * Power Management:
687 * This function does nothing for now but must be present for pm to work
688 */
689static int mt9t031_runtime_suspend(struct device *dev)
690{
691	return 0;
692}
693
694/*
695 * Power Management:
696 * COLUMN_ADDRESS_MODE and ROW_ADDRESS_MODE are not rewritten if unchanged
697 * they are however changed at reset if the platform hook is present
698 * thus we rewrite them with the values stored by the driver
699 */
700static int mt9t031_runtime_resume(struct device *dev)
701{
702	struct video_device *vdev = to_video_device(dev);
703	struct soc_camera_device *icd = container_of(vdev->parent,
704		struct soc_camera_device, dev);
705	struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
706	struct i2c_client *client = v4l2_get_subdevdata(sd);
707	struct mt9t031 *mt9t031 = to_mt9t031(client);
708
709	int ret;
710	u16 xbin, ybin;
711
712	xbin = min(mt9t031->xskip, (u16)3);
713	ybin = min(mt9t031->yskip, (u16)3);
714
715	ret = reg_write(client, MT9T031_COLUMN_ADDRESS_MODE,
716		((xbin - 1) << 4) | (mt9t031->xskip - 1));
717	if (ret < 0)
718		return ret;
719
720	ret = reg_write(client, MT9T031_ROW_ADDRESS_MODE,
721		((ybin - 1) << 4) | (mt9t031->yskip - 1));
722	if (ret < 0)
723		return ret;
724
725	return 0;
726}
727
728static struct dev_pm_ops mt9t031_dev_pm_ops = {
729	.runtime_suspend	= mt9t031_runtime_suspend,
730	.runtime_resume		= mt9t031_runtime_resume,
731};
732
733static struct device_type mt9t031_dev_type = {
734	.name	= "MT9T031",
735	.pm	= &mt9t031_dev_pm_ops,
736};
737
738/*
739 * Interface active, can use i2c. If it fails, it can indeed mean, that
740 * this wasn't our capture interface, so, we wait for the right one
741 */
742static int mt9t031_video_probe(struct i2c_client *client)
743{
744	struct mt9t031 *mt9t031 = to_mt9t031(client);
745	struct video_device *vdev = soc_camera_i2c_to_vdev(client);
746	s32 data;
747	int ret;
748
749	/* Enable the chip */
750	data = reg_write(client, MT9T031_CHIP_ENABLE, 1);
751	dev_dbg(&client->dev, "write: %d\n", data);
752
753	/* Read out the chip version register */
754	data = reg_read(client, MT9T031_CHIP_VERSION);
755
756	switch (data) {
757	case 0x1621:
758		mt9t031->model = V4L2_IDENT_MT9T031;
759		break;
760	default:
761		dev_err(&client->dev,
762			"No MT9T031 chip detected, register read %x\n", data);
763		return -ENODEV;
764	}
765
766	dev_info(&client->dev, "Detected a MT9T031 chip ID %x\n", data);
767
768	ret = mt9t031_idle(client);
769	if (ret < 0)
770		dev_err(&client->dev, "Failed to initialise the camera\n");
771	else
772		vdev->dev.type = &mt9t031_dev_type;
773
774	/* mt9t031_idle() has reset the chip to default. */
775	mt9t031->exposure = 255;
776	mt9t031->gain = 64;
777
778	return ret;
779}
780
781static int mt9t031_g_skip_top_lines(struct v4l2_subdev *sd, u32 *lines)
782{
783	struct i2c_client *client = v4l2_get_subdevdata(sd);
784	struct mt9t031 *mt9t031 = to_mt9t031(client);
785
786	*lines = mt9t031->y_skip_top;
787
788	return 0;
789}
790
791static struct v4l2_subdev_core_ops mt9t031_subdev_core_ops = {
792	.g_ctrl		= mt9t031_g_ctrl,
793	.s_ctrl		= mt9t031_s_ctrl,
794	.g_chip_ident	= mt9t031_g_chip_ident,
795#ifdef CONFIG_VIDEO_ADV_DEBUG
796	.g_register	= mt9t031_g_register,
797	.s_register	= mt9t031_s_register,
798#endif
799};
800
801static int mt9t031_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
802			    enum v4l2_mbus_pixelcode *code)
803{
804	if (index)
805		return -EINVAL;
806
807	*code = V4L2_MBUS_FMT_SBGGR10_1X10;
808	return 0;
809}
810
811static struct v4l2_subdev_video_ops mt9t031_subdev_video_ops = {
812	.s_stream	= mt9t031_s_stream,
813	.s_mbus_fmt	= mt9t031_s_fmt,
814	.g_mbus_fmt	= mt9t031_g_fmt,
815	.try_mbus_fmt	= mt9t031_try_fmt,
816	.s_crop		= mt9t031_s_crop,
817	.g_crop		= mt9t031_g_crop,
818	.cropcap	= mt9t031_cropcap,
819	.enum_mbus_fmt	= mt9t031_enum_fmt,
820};
821
822static struct v4l2_subdev_sensor_ops mt9t031_subdev_sensor_ops = {
823	.g_skip_top_lines	= mt9t031_g_skip_top_lines,
824};
825
826static struct v4l2_subdev_ops mt9t031_subdev_ops = {
827	.core	= &mt9t031_subdev_core_ops,
828	.video	= &mt9t031_subdev_video_ops,
829	.sensor	= &mt9t031_subdev_sensor_ops,
830};
831
832static int mt9t031_probe(struct i2c_client *client,
833			 const struct i2c_device_id *did)
834{
835	struct mt9t031 *mt9t031;
836	struct soc_camera_device *icd = client->dev.platform_data;
837	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
838	int ret;
839
840	if (icd) {
841		struct soc_camera_link *icl = to_soc_camera_link(icd);
842		if (!icl) {
843			dev_err(&client->dev, "MT9T031 driver needs platform data\n");
844			return -EINVAL;
845		}
846
847		icd->ops = &mt9t031_ops;
848	}
849
850	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
851		dev_warn(&adapter->dev,
852			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
853		return -EIO;
854	}
855
856	mt9t031 = kzalloc(sizeof(struct mt9t031), GFP_KERNEL);
857	if (!mt9t031)
858		return -ENOMEM;
859
860	v4l2_i2c_subdev_init(&mt9t031->subdev, client, &mt9t031_subdev_ops);
861
862	mt9t031->y_skip_top	= 0;
863	mt9t031->rect.left	= MT9T031_COLUMN_SKIP;
864	mt9t031->rect.top	= MT9T031_ROW_SKIP;
865	mt9t031->rect.width	= MT9T031_MAX_WIDTH;
866	mt9t031->rect.height	= MT9T031_MAX_HEIGHT;
867
868	/*
869	 * Simulated autoexposure. If enabled, we calculate shutter width
870	 * ourselves in the driver based on vertical blanking and frame width
871	 */
872	mt9t031->autoexposure = 1;
873
874	mt9t031->xskip = 1;
875	mt9t031->yskip = 1;
876
877	mt9t031_idle(client);
878
879	ret = mt9t031_video_probe(client);
880
881	mt9t031_disable(client);
882
883	if (ret) {
884		if (icd)
885			icd->ops = NULL;
886		kfree(mt9t031);
887	}
888
889	return ret;
890}
891
892static int mt9t031_remove(struct i2c_client *client)
893{
894	struct mt9t031 *mt9t031 = to_mt9t031(client);
895	struct soc_camera_device *icd = client->dev.platform_data;
896
897	if (icd)
898		icd->ops = NULL;
899	kfree(mt9t031);
900
901	return 0;
902}
903
904static const struct i2c_device_id mt9t031_id[] = {
905	{ "mt9t031", 0 },
906	{ }
907};
908MODULE_DEVICE_TABLE(i2c, mt9t031_id);
909
910static struct i2c_driver mt9t031_i2c_driver = {
911	.driver = {
912		.name = "mt9t031",
913	},
914	.probe		= mt9t031_probe,
915	.remove		= mt9t031_remove,
916	.id_table	= mt9t031_id,
917};
918
919static int __init mt9t031_mod_init(void)
920{
921	return i2c_add_driver(&mt9t031_i2c_driver);
922}
923
924static void __exit mt9t031_mod_exit(void)
925{
926	i2c_del_driver(&mt9t031_i2c_driver);
927}
928
929module_init(mt9t031_mod_init);
930module_exit(mt9t031_mod_exit);
931
932MODULE_DESCRIPTION("Micron MT9T031 Camera driver");
933MODULE_AUTHOR("Guennadi Liakhovetski <lg@denx.de>");
934MODULE_LICENSE("GPL v2");