1 /*
2  * Driver for MT9M111/MT9M112/MT9M131 CMOS Image Sensor from Micron/Aptina
3  *
4  * Copyright (C) 2008, Robert Jarzmik <robert.jarzmik@free.fr>
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 #include <linux/videodev2.h>
11 #include <linux/slab.h>
12 #include <linux/i2c.h>
13 #include <linux/log2.h>
14 #include <linux/gpio.h>
15 #include <linux/delay.h>
16 #include <linux/v4l2-mediabus.h>
17 #include <linux/module.h>
18 
19 #include <media/soc_camera.h>
20 #include <media/v4l2-common.h>
21 #include <media/v4l2-ctrls.h>
22 #include <media/v4l2-chip-ident.h>
23 
24 /*
25  * MT9M111, MT9M112 and MT9M131:
26  * i2c address is 0x48 or 0x5d (depending on SADDR pin)
27  * The platform has to define i2c_board_info and call i2c_register_board_info()
28  */
29 
30 /*
31  * Sensor core register addresses (0x000..0x0ff)
32  */
33 #define MT9M111_CHIP_VERSION		0x000
34 #define MT9M111_ROW_START		0x001
35 #define MT9M111_COLUMN_START		0x002
36 #define MT9M111_WINDOW_HEIGHT		0x003
37 #define MT9M111_WINDOW_WIDTH		0x004
38 #define MT9M111_HORIZONTAL_BLANKING_B	0x005
39 #define MT9M111_VERTICAL_BLANKING_B	0x006
40 #define MT9M111_HORIZONTAL_BLANKING_A	0x007
41 #define MT9M111_VERTICAL_BLANKING_A	0x008
42 #define MT9M111_SHUTTER_WIDTH		0x009
43 #define MT9M111_ROW_SPEED		0x00a
44 #define MT9M111_EXTRA_DELAY		0x00b
45 #define MT9M111_SHUTTER_DELAY		0x00c
46 #define MT9M111_RESET			0x00d
47 #define MT9M111_READ_MODE_B		0x020
48 #define MT9M111_READ_MODE_A		0x021
49 #define MT9M111_FLASH_CONTROL		0x023
50 #define MT9M111_GREEN1_GAIN		0x02b
51 #define MT9M111_BLUE_GAIN		0x02c
52 #define MT9M111_RED_GAIN		0x02d
53 #define MT9M111_GREEN2_GAIN		0x02e
54 #define MT9M111_GLOBAL_GAIN		0x02f
55 #define MT9M111_CONTEXT_CONTROL		0x0c8
56 #define MT9M111_PAGE_MAP		0x0f0
57 #define MT9M111_BYTE_WISE_ADDR		0x0f1
58 
59 #define MT9M111_RESET_SYNC_CHANGES	(1 << 15)
60 #define MT9M111_RESET_RESTART_BAD_FRAME	(1 << 9)
61 #define MT9M111_RESET_SHOW_BAD_FRAMES	(1 << 8)
62 #define MT9M111_RESET_RESET_SOC		(1 << 5)
63 #define MT9M111_RESET_OUTPUT_DISABLE	(1 << 4)
64 #define MT9M111_RESET_CHIP_ENABLE	(1 << 3)
65 #define MT9M111_RESET_ANALOG_STANDBY	(1 << 2)
66 #define MT9M111_RESET_RESTART_FRAME	(1 << 1)
67 #define MT9M111_RESET_RESET_MODE	(1 << 0)
68 
69 #define MT9M111_RM_FULL_POWER_RD	(0 << 10)
70 #define MT9M111_RM_LOW_POWER_RD		(1 << 10)
71 #define MT9M111_RM_COL_SKIP_4X		(1 << 5)
72 #define MT9M111_RM_ROW_SKIP_4X		(1 << 4)
73 #define MT9M111_RM_COL_SKIP_2X		(1 << 3)
74 #define MT9M111_RM_ROW_SKIP_2X		(1 << 2)
75 #define MT9M111_RMB_MIRROR_COLS		(1 << 1)
76 #define MT9M111_RMB_MIRROR_ROWS		(1 << 0)
77 #define MT9M111_CTXT_CTRL_RESTART	(1 << 15)
78 #define MT9M111_CTXT_CTRL_DEFECTCOR_B	(1 << 12)
79 #define MT9M111_CTXT_CTRL_RESIZE_B	(1 << 10)
80 #define MT9M111_CTXT_CTRL_CTRL2_B	(1 << 9)
81 #define MT9M111_CTXT_CTRL_GAMMA_B	(1 << 8)
82 #define MT9M111_CTXT_CTRL_XENON_EN	(1 << 7)
83 #define MT9M111_CTXT_CTRL_READ_MODE_B	(1 << 3)
84 #define MT9M111_CTXT_CTRL_LED_FLASH_EN	(1 << 2)
85 #define MT9M111_CTXT_CTRL_VBLANK_SEL_B	(1 << 1)
86 #define MT9M111_CTXT_CTRL_HBLANK_SEL_B	(1 << 0)
87 
88 /*
89  * Colorpipe register addresses (0x100..0x1ff)
90  */
91 #define MT9M111_OPER_MODE_CTRL		0x106
92 #define MT9M111_OUTPUT_FORMAT_CTRL	0x108
93 #define MT9M111_REDUCER_XZOOM_B		0x1a0
94 #define MT9M111_REDUCER_XSIZE_B		0x1a1
95 #define MT9M111_REDUCER_YZOOM_B		0x1a3
96 #define MT9M111_REDUCER_YSIZE_B		0x1a4
97 #define MT9M111_REDUCER_XZOOM_A		0x1a6
98 #define MT9M111_REDUCER_XSIZE_A		0x1a7
99 #define MT9M111_REDUCER_YZOOM_A		0x1a9
100 #define MT9M111_REDUCER_YSIZE_A		0x1aa
101 
102 #define MT9M111_OUTPUT_FORMAT_CTRL2_A	0x13a
103 #define MT9M111_OUTPUT_FORMAT_CTRL2_B	0x19b
104 
105 #define MT9M111_OPMODE_AUTOEXPO_EN	(1 << 14)
106 #define MT9M111_OPMODE_AUTOWHITEBAL_EN	(1 << 1)
107 #define MT9M111_OUTFMT_FLIP_BAYER_COL	(1 << 9)
108 #define MT9M111_OUTFMT_FLIP_BAYER_ROW	(1 << 8)
109 #define MT9M111_OUTFMT_PROCESSED_BAYER	(1 << 14)
110 #define MT9M111_OUTFMT_BYPASS_IFP	(1 << 10)
111 #define MT9M111_OUTFMT_INV_PIX_CLOCK	(1 << 9)
112 #define MT9M111_OUTFMT_RGB		(1 << 8)
113 #define MT9M111_OUTFMT_RGB565		(0 << 6)
114 #define MT9M111_OUTFMT_RGB555		(1 << 6)
115 #define MT9M111_OUTFMT_RGB444x		(2 << 6)
116 #define MT9M111_OUTFMT_RGBx444		(3 << 6)
117 #define MT9M111_OUTFMT_TST_RAMP_OFF	(0 << 4)
118 #define MT9M111_OUTFMT_TST_RAMP_COL	(1 << 4)
119 #define MT9M111_OUTFMT_TST_RAMP_ROW	(2 << 4)
120 #define MT9M111_OUTFMT_TST_RAMP_FRAME	(3 << 4)
121 #define MT9M111_OUTFMT_SHIFT_3_UP	(1 << 3)
122 #define MT9M111_OUTFMT_AVG_CHROMA	(1 << 2)
123 #define MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN	(1 << 1)
124 #define MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B	(1 << 0)
125 
126 /*
127  * Camera control register addresses (0x200..0x2ff not implemented)
128  */
129 
130 #define reg_read(reg) mt9m111_reg_read(client, MT9M111_##reg)
131 #define reg_write(reg, val) mt9m111_reg_write(client, MT9M111_##reg, (val))
132 #define reg_set(reg, val) mt9m111_reg_set(client, MT9M111_##reg, (val))
133 #define reg_clear(reg, val) mt9m111_reg_clear(client, MT9M111_##reg, (val))
134 #define reg_mask(reg, val, mask) mt9m111_reg_mask(client, MT9M111_##reg, \
135 		(val), (mask))
136 
137 #define MT9M111_MIN_DARK_ROWS	8
138 #define MT9M111_MIN_DARK_COLS	26
139 #define MT9M111_MAX_HEIGHT	1024
140 #define MT9M111_MAX_WIDTH	1280
141 
142 struct mt9m111_context {
143 	u16 read_mode;
144 	u16 blanking_h;
145 	u16 blanking_v;
146 	u16 reducer_xzoom;
147 	u16 reducer_yzoom;
148 	u16 reducer_xsize;
149 	u16 reducer_ysize;
150 	u16 output_fmt_ctrl2;
151 	u16 control;
152 };
153 
154 static struct mt9m111_context context_a = {
155 	.read_mode		= MT9M111_READ_MODE_A,
156 	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_A,
157 	.blanking_v		= MT9M111_VERTICAL_BLANKING_A,
158 	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_A,
159 	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_A,
160 	.reducer_xsize		= MT9M111_REDUCER_XSIZE_A,
161 	.reducer_ysize		= MT9M111_REDUCER_YSIZE_A,
162 	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_A,
163 	.control		= MT9M111_CTXT_CTRL_RESTART,
164 };
165 
166 static struct mt9m111_context context_b = {
167 	.read_mode		= MT9M111_READ_MODE_B,
168 	.blanking_h		= MT9M111_HORIZONTAL_BLANKING_B,
169 	.blanking_v		= MT9M111_VERTICAL_BLANKING_B,
170 	.reducer_xzoom		= MT9M111_REDUCER_XZOOM_B,
171 	.reducer_yzoom		= MT9M111_REDUCER_YZOOM_B,
172 	.reducer_xsize		= MT9M111_REDUCER_XSIZE_B,
173 	.reducer_ysize		= MT9M111_REDUCER_YSIZE_B,
174 	.output_fmt_ctrl2	= MT9M111_OUTPUT_FORMAT_CTRL2_B,
175 	.control		= MT9M111_CTXT_CTRL_RESTART |
176 		MT9M111_CTXT_CTRL_DEFECTCOR_B | MT9M111_CTXT_CTRL_RESIZE_B |
177 		MT9M111_CTXT_CTRL_CTRL2_B | MT9M111_CTXT_CTRL_GAMMA_B |
178 		MT9M111_CTXT_CTRL_READ_MODE_B | MT9M111_CTXT_CTRL_VBLANK_SEL_B |
179 		MT9M111_CTXT_CTRL_HBLANK_SEL_B,
180 };
181 
182 /* MT9M111 has only one fixed colorspace per pixelcode */
183 struct mt9m111_datafmt {
184 	enum v4l2_mbus_pixelcode	code;
185 	enum v4l2_colorspace		colorspace;
186 };
187 
188 static const struct mt9m111_datafmt mt9m111_colour_fmts[] = {
189 	{V4L2_MBUS_FMT_YUYV8_2X8, V4L2_COLORSPACE_JPEG},
190 	{V4L2_MBUS_FMT_YVYU8_2X8, V4L2_COLORSPACE_JPEG},
191 	{V4L2_MBUS_FMT_UYVY8_2X8, V4L2_COLORSPACE_JPEG},
192 	{V4L2_MBUS_FMT_VYUY8_2X8, V4L2_COLORSPACE_JPEG},
193 	{V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
194 	{V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE, V4L2_COLORSPACE_SRGB},
195 	{V4L2_MBUS_FMT_RGB565_2X8_LE, V4L2_COLORSPACE_SRGB},
196 	{V4L2_MBUS_FMT_RGB565_2X8_BE, V4L2_COLORSPACE_SRGB},
197 	{V4L2_MBUS_FMT_BGR565_2X8_LE, V4L2_COLORSPACE_SRGB},
198 	{V4L2_MBUS_FMT_BGR565_2X8_BE, V4L2_COLORSPACE_SRGB},
199 	{V4L2_MBUS_FMT_SBGGR8_1X8, V4L2_COLORSPACE_SRGB},
200 	{V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE, V4L2_COLORSPACE_SRGB},
201 };
202 
203 struct mt9m111 {
204 	struct v4l2_subdev subdev;
205 	struct v4l2_ctrl_handler hdl;
206 	struct v4l2_ctrl *gain;
207 	int model;	/* V4L2_IDENT_MT9M111 or V4L2_IDENT_MT9M112 code
208 			 * from v4l2-chip-ident.h */
209 	struct mt9m111_context *ctx;
210 	struct v4l2_rect rect;	/* cropping rectangle */
211 	int width;		/* output */
212 	int height;		/* sizes */
213 	struct mutex power_lock; /* lock to protect power_count */
214 	int power_count;
215 	const struct mt9m111_datafmt *fmt;
216 	int lastpage;	/* PageMap cache value */
217 	unsigned char datawidth;
218 };
219 
220 /* Find a data format by a pixel code */
mt9m111_find_datafmt(struct mt9m111 * mt9m111,enum v4l2_mbus_pixelcode code)221 static const struct mt9m111_datafmt *mt9m111_find_datafmt(struct mt9m111 *mt9m111,
222 						enum v4l2_mbus_pixelcode code)
223 {
224 	int i;
225 	for (i = 0; i < ARRAY_SIZE(mt9m111_colour_fmts); i++)
226 		if (mt9m111_colour_fmts[i].code == code)
227 			return mt9m111_colour_fmts + i;
228 
229 	return mt9m111->fmt;
230 }
231 
to_mt9m111(const struct i2c_client * client)232 static struct mt9m111 *to_mt9m111(const struct i2c_client *client)
233 {
234 	return container_of(i2c_get_clientdata(client), struct mt9m111, subdev);
235 }
236 
reg_page_map_set(struct i2c_client * client,const u16 reg)237 static int reg_page_map_set(struct i2c_client *client, const u16 reg)
238 {
239 	int ret;
240 	u16 page;
241 	struct mt9m111 *mt9m111 = to_mt9m111(client);
242 
243 	page = (reg >> 8);
244 	if (page == mt9m111->lastpage)
245 		return 0;
246 	if (page > 2)
247 		return -EINVAL;
248 
249 	ret = i2c_smbus_write_word_swapped(client, MT9M111_PAGE_MAP, page);
250 	if (!ret)
251 		mt9m111->lastpage = page;
252 	return ret;
253 }
254 
mt9m111_reg_read(struct i2c_client * client,const u16 reg)255 static int mt9m111_reg_read(struct i2c_client *client, const u16 reg)
256 {
257 	int ret;
258 
259 	ret = reg_page_map_set(client, reg);
260 	if (!ret)
261 		ret = i2c_smbus_read_word_swapped(client, reg & 0xff);
262 
263 	dev_dbg(&client->dev, "read  reg.%03x -> %04x\n", reg, ret);
264 	return ret;
265 }
266 
mt9m111_reg_write(struct i2c_client * client,const u16 reg,const u16 data)267 static int mt9m111_reg_write(struct i2c_client *client, const u16 reg,
268 			     const u16 data)
269 {
270 	int ret;
271 
272 	ret = reg_page_map_set(client, reg);
273 	if (!ret)
274 		ret = i2c_smbus_write_word_swapped(client, reg & 0xff, data);
275 	dev_dbg(&client->dev, "write reg.%03x = %04x -> %d\n", reg, data, ret);
276 	return ret;
277 }
278 
mt9m111_reg_set(struct i2c_client * client,const u16 reg,const u16 data)279 static int mt9m111_reg_set(struct i2c_client *client, const u16 reg,
280 			   const u16 data)
281 {
282 	int ret;
283 
284 	ret = mt9m111_reg_read(client, reg);
285 	if (ret >= 0)
286 		ret = mt9m111_reg_write(client, reg, ret | data);
287 	return ret;
288 }
289 
mt9m111_reg_clear(struct i2c_client * client,const u16 reg,const u16 data)290 static int mt9m111_reg_clear(struct i2c_client *client, const u16 reg,
291 			     const u16 data)
292 {
293 	int ret;
294 
295 	ret = mt9m111_reg_read(client, reg);
296 	if (ret >= 0)
297 		ret = mt9m111_reg_write(client, reg, ret & ~data);
298 	return ret;
299 }
300 
mt9m111_reg_mask(struct i2c_client * client,const u16 reg,const u16 data,const u16 mask)301 static int mt9m111_reg_mask(struct i2c_client *client, const u16 reg,
302 			    const u16 data, const u16 mask)
303 {
304 	int ret;
305 
306 	ret = mt9m111_reg_read(client, reg);
307 	if (ret >= 0)
308 		ret = mt9m111_reg_write(client, reg, (ret & ~mask) | data);
309 	return ret;
310 }
311 
mt9m111_set_context(struct mt9m111 * mt9m111,struct mt9m111_context * ctx)312 static int mt9m111_set_context(struct mt9m111 *mt9m111,
313 			       struct mt9m111_context *ctx)
314 {
315 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
316 	return reg_write(CONTEXT_CONTROL, ctx->control);
317 }
318 
mt9m111_setup_rect_ctx(struct mt9m111 * mt9m111,struct mt9m111_context * ctx,struct v4l2_rect * rect,unsigned int width,unsigned int height)319 static int mt9m111_setup_rect_ctx(struct mt9m111 *mt9m111,
320 			struct mt9m111_context *ctx, struct v4l2_rect *rect,
321 			unsigned int width, unsigned int height)
322 {
323 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
324 	int ret = mt9m111_reg_write(client, ctx->reducer_xzoom, rect->width);
325 	if (!ret)
326 		ret = mt9m111_reg_write(client, ctx->reducer_yzoom, rect->height);
327 	if (!ret)
328 		ret = mt9m111_reg_write(client, ctx->reducer_xsize, width);
329 	if (!ret)
330 		ret = mt9m111_reg_write(client, ctx->reducer_ysize, height);
331 	return ret;
332 }
333 
mt9m111_setup_geometry(struct mt9m111 * mt9m111,struct v4l2_rect * rect,int width,int height,enum v4l2_mbus_pixelcode code)334 static int mt9m111_setup_geometry(struct mt9m111 *mt9m111, struct v4l2_rect *rect,
335 			int width, int height, enum v4l2_mbus_pixelcode code)
336 {
337 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
338 	int ret;
339 
340 	ret = reg_write(COLUMN_START, rect->left);
341 	if (!ret)
342 		ret = reg_write(ROW_START, rect->top);
343 
344 	if (!ret)
345 		ret = reg_write(WINDOW_WIDTH, rect->width);
346 	if (!ret)
347 		ret = reg_write(WINDOW_HEIGHT, rect->height);
348 
349 	if (code != V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE) {
350 		/* IFP in use, down-scaling possible */
351 		if (!ret)
352 			ret = mt9m111_setup_rect_ctx(mt9m111, &context_b,
353 						     rect, width, height);
354 		if (!ret)
355 			ret = mt9m111_setup_rect_ctx(mt9m111, &context_a,
356 						     rect, width, height);
357 	}
358 
359 	dev_dbg(&client->dev, "%s(%x): %ux%u@%u:%u -> %ux%u = %d\n",
360 		__func__, code, rect->width, rect->height, rect->left, rect->top,
361 		width, height, ret);
362 
363 	return ret;
364 }
365 
mt9m111_enable(struct mt9m111 * mt9m111)366 static int mt9m111_enable(struct mt9m111 *mt9m111)
367 {
368 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
369 	return reg_write(RESET, MT9M111_RESET_CHIP_ENABLE);
370 }
371 
mt9m111_reset(struct mt9m111 * mt9m111)372 static int mt9m111_reset(struct mt9m111 *mt9m111)
373 {
374 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
375 	int ret;
376 
377 	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
378 	if (!ret)
379 		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC);
380 	if (!ret)
381 		ret = reg_clear(RESET, MT9M111_RESET_RESET_MODE
382 				| MT9M111_RESET_RESET_SOC);
383 
384 	return ret;
385 }
386 
mt9m111_s_crop(struct v4l2_subdev * sd,struct v4l2_crop * a)387 static int mt9m111_s_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
388 {
389 	struct v4l2_rect rect = a->c;
390 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
391 	int width, height;
392 	int ret;
393 
394 	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
395 		return -EINVAL;
396 
397 	if (mt9m111->fmt->code == V4L2_MBUS_FMT_SBGGR8_1X8 ||
398 	    mt9m111->fmt->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE) {
399 		/* Bayer format - even size lengths */
400 		rect.width	= ALIGN(rect.width, 2);
401 		rect.height	= ALIGN(rect.height, 2);
402 		/* Let the user play with the starting pixel */
403 	}
404 
405 	/* FIXME: the datasheet doesn't specify minimum sizes */
406 	soc_camera_limit_side(&rect.left, &rect.width,
407 		     MT9M111_MIN_DARK_COLS, 2, MT9M111_MAX_WIDTH);
408 
409 	soc_camera_limit_side(&rect.top, &rect.height,
410 		     MT9M111_MIN_DARK_ROWS, 2, MT9M111_MAX_HEIGHT);
411 
412 	width = min(mt9m111->width, rect.width);
413 	height = min(mt9m111->height, rect.height);
414 
415 	ret = mt9m111_setup_geometry(mt9m111, &rect, width, height, mt9m111->fmt->code);
416 	if (!ret) {
417 		mt9m111->rect = rect;
418 		mt9m111->width = width;
419 		mt9m111->height = height;
420 	}
421 
422 	return ret;
423 }
424 
mt9m111_g_crop(struct v4l2_subdev * sd,struct v4l2_crop * a)425 static int mt9m111_g_crop(struct v4l2_subdev *sd, struct v4l2_crop *a)
426 {
427 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
428 
429 	a->c	= mt9m111->rect;
430 	a->type	= V4L2_BUF_TYPE_VIDEO_CAPTURE;
431 
432 	return 0;
433 }
434 
mt9m111_cropcap(struct v4l2_subdev * sd,struct v4l2_cropcap * a)435 static int mt9m111_cropcap(struct v4l2_subdev *sd, struct v4l2_cropcap *a)
436 {
437 	if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
438 		return -EINVAL;
439 
440 	a->bounds.left			= MT9M111_MIN_DARK_COLS;
441 	a->bounds.top			= MT9M111_MIN_DARK_ROWS;
442 	a->bounds.width			= MT9M111_MAX_WIDTH;
443 	a->bounds.height		= MT9M111_MAX_HEIGHT;
444 	a->defrect			= a->bounds;
445 	a->pixelaspect.numerator	= 1;
446 	a->pixelaspect.denominator	= 1;
447 
448 	return 0;
449 }
450 
mt9m111_g_fmt(struct v4l2_subdev * sd,struct v4l2_mbus_framefmt * mf)451 static int mt9m111_g_fmt(struct v4l2_subdev *sd,
452 			 struct v4l2_mbus_framefmt *mf)
453 {
454 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
455 
456 	mf->width	= mt9m111->width;
457 	mf->height	= mt9m111->height;
458 	mf->code	= mt9m111->fmt->code;
459 	mf->colorspace	= mt9m111->fmt->colorspace;
460 	mf->field	= V4L2_FIELD_NONE;
461 
462 	return 0;
463 }
464 
mt9m111_set_pixfmt(struct mt9m111 * mt9m111,enum v4l2_mbus_pixelcode code)465 static int mt9m111_set_pixfmt(struct mt9m111 *mt9m111,
466 			      enum v4l2_mbus_pixelcode code)
467 {
468 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
469 	u16 data_outfmt2, mask_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
470 		MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB |
471 		MT9M111_OUTFMT_RGB565 | MT9M111_OUTFMT_RGB555 |
472 		MT9M111_OUTFMT_RGB444x | MT9M111_OUTFMT_RGBx444 |
473 		MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
474 		MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
475 	int ret;
476 
477 	switch (code) {
478 	case V4L2_MBUS_FMT_SBGGR8_1X8:
479 		data_outfmt2 = MT9M111_OUTFMT_PROCESSED_BAYER |
480 			MT9M111_OUTFMT_RGB;
481 		break;
482 	case V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE:
483 		data_outfmt2 = MT9M111_OUTFMT_BYPASS_IFP | MT9M111_OUTFMT_RGB;
484 		break;
485 	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
486 		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555 |
487 			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
488 		break;
489 	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE:
490 		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB555;
491 		break;
492 	case V4L2_MBUS_FMT_RGB565_2X8_LE:
493 		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
494 			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
495 		break;
496 	case V4L2_MBUS_FMT_RGB565_2X8_BE:
497 		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565;
498 		break;
499 	case V4L2_MBUS_FMT_BGR565_2X8_BE:
500 		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
501 			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
502 		break;
503 	case V4L2_MBUS_FMT_BGR565_2X8_LE:
504 		data_outfmt2 = MT9M111_OUTFMT_RGB | MT9M111_OUTFMT_RGB565 |
505 			MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
506 			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
507 		break;
508 	case V4L2_MBUS_FMT_UYVY8_2X8:
509 		data_outfmt2 = 0;
510 		break;
511 	case V4L2_MBUS_FMT_VYUY8_2X8:
512 		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
513 		break;
514 	case V4L2_MBUS_FMT_YUYV8_2X8:
515 		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN;
516 		break;
517 	case V4L2_MBUS_FMT_YVYU8_2X8:
518 		data_outfmt2 = MT9M111_OUTFMT_SWAP_YCbCr_C_Y_RGB_EVEN |
519 			MT9M111_OUTFMT_SWAP_YCbCr_Cb_Cr_RGB_R_B;
520 		break;
521 	default:
522 		dev_err(&client->dev, "Pixel format not handled: %x\n", code);
523 		return -EINVAL;
524 	}
525 
526 	ret = mt9m111_reg_mask(client, context_a.output_fmt_ctrl2,
527 			       data_outfmt2, mask_outfmt2);
528 	if (!ret)
529 		ret = mt9m111_reg_mask(client, context_b.output_fmt_ctrl2,
530 				       data_outfmt2, mask_outfmt2);
531 
532 	return ret;
533 }
534 
mt9m111_try_fmt(struct v4l2_subdev * sd,struct v4l2_mbus_framefmt * mf)535 static int mt9m111_try_fmt(struct v4l2_subdev *sd,
536 			   struct v4l2_mbus_framefmt *mf)
537 {
538 	struct i2c_client *client = v4l2_get_subdevdata(sd);
539 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
540 	const struct mt9m111_datafmt *fmt;
541 	struct v4l2_rect *rect = &mt9m111->rect;
542 	bool bayer;
543 
544 	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
545 
546 	bayer = fmt->code == V4L2_MBUS_FMT_SBGGR8_1X8 ||
547 		fmt->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE;
548 
549 	/*
550 	 * With Bayer format enforce even side lengths, but let the user play
551 	 * with the starting pixel
552 	 */
553 	if (bayer) {
554 		rect->width = ALIGN(rect->width, 2);
555 		rect->height = ALIGN(rect->height, 2);
556 	}
557 
558 	if (fmt->code == V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE) {
559 		/* IFP bypass mode, no scaling */
560 		mf->width = rect->width;
561 		mf->height = rect->height;
562 	} else {
563 		/* No upscaling */
564 		if (mf->width > rect->width)
565 			mf->width = rect->width;
566 		if (mf->height > rect->height)
567 			mf->height = rect->height;
568 	}
569 
570 	dev_dbg(&client->dev, "%s(): %ux%u, code=%x\n", __func__,
571 		mf->width, mf->height, fmt->code);
572 
573 	mf->code = fmt->code;
574 	mf->colorspace = fmt->colorspace;
575 
576 	return 0;
577 }
578 
mt9m111_s_fmt(struct v4l2_subdev * sd,struct v4l2_mbus_framefmt * mf)579 static int mt9m111_s_fmt(struct v4l2_subdev *sd,
580 			 struct v4l2_mbus_framefmt *mf)
581 {
582 	const struct mt9m111_datafmt *fmt;
583 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
584 	struct v4l2_rect *rect = &mt9m111->rect;
585 	int ret;
586 
587 	mt9m111_try_fmt(sd, mf);
588 	fmt = mt9m111_find_datafmt(mt9m111, mf->code);
589 	/* try_fmt() guarantees fmt != NULL && fmt->code == mf->code */
590 
591 	ret = mt9m111_setup_geometry(mt9m111, rect, mf->width, mf->height, mf->code);
592 	if (!ret)
593 		ret = mt9m111_set_pixfmt(mt9m111, mf->code);
594 	if (!ret) {
595 		mt9m111->width	= mf->width;
596 		mt9m111->height	= mf->height;
597 		mt9m111->fmt	= fmt;
598 	}
599 
600 	return ret;
601 }
602 
mt9m111_g_chip_ident(struct v4l2_subdev * sd,struct v4l2_dbg_chip_ident * id)603 static int mt9m111_g_chip_ident(struct v4l2_subdev *sd,
604 				struct v4l2_dbg_chip_ident *id)
605 {
606 	struct i2c_client *client = v4l2_get_subdevdata(sd);
607 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
608 
609 	if (id->match.type != V4L2_CHIP_MATCH_I2C_ADDR)
610 		return -EINVAL;
611 
612 	if (id->match.addr != client->addr)
613 		return -ENODEV;
614 
615 	id->ident	= mt9m111->model;
616 	id->revision	= 0;
617 
618 	return 0;
619 }
620 
621 #ifdef CONFIG_VIDEO_ADV_DEBUG
mt9m111_g_register(struct v4l2_subdev * sd,struct v4l2_dbg_register * reg)622 static int mt9m111_g_register(struct v4l2_subdev *sd,
623 			      struct v4l2_dbg_register *reg)
624 {
625 	struct i2c_client *client = v4l2_get_subdevdata(sd);
626 	int val;
627 
628 	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0x2ff)
629 		return -EINVAL;
630 	if (reg->match.addr != client->addr)
631 		return -ENODEV;
632 
633 	val = mt9m111_reg_read(client, reg->reg);
634 	reg->size = 2;
635 	reg->val = (u64)val;
636 
637 	if (reg->val > 0xffff)
638 		return -EIO;
639 
640 	return 0;
641 }
642 
mt9m111_s_register(struct v4l2_subdev * sd,struct v4l2_dbg_register * reg)643 static int mt9m111_s_register(struct v4l2_subdev *sd,
644 			      struct v4l2_dbg_register *reg)
645 {
646 	struct i2c_client *client = v4l2_get_subdevdata(sd);
647 
648 	if (reg->match.type != V4L2_CHIP_MATCH_I2C_ADDR || reg->reg > 0x2ff)
649 		return -EINVAL;
650 
651 	if (reg->match.addr != client->addr)
652 		return -ENODEV;
653 
654 	if (mt9m111_reg_write(client, reg->reg, reg->val) < 0)
655 		return -EIO;
656 
657 	return 0;
658 }
659 #endif
660 
mt9m111_set_flip(struct mt9m111 * mt9m111,int flip,int mask)661 static int mt9m111_set_flip(struct mt9m111 *mt9m111, int flip, int mask)
662 {
663 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
664 	int ret;
665 
666 	if (flip)
667 		ret = mt9m111_reg_set(client, mt9m111->ctx->read_mode, mask);
668 	else
669 		ret = mt9m111_reg_clear(client, mt9m111->ctx->read_mode, mask);
670 
671 	return ret;
672 }
673 
mt9m111_get_global_gain(struct mt9m111 * mt9m111)674 static int mt9m111_get_global_gain(struct mt9m111 *mt9m111)
675 {
676 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
677 	int data;
678 
679 	data = reg_read(GLOBAL_GAIN);
680 	if (data >= 0)
681 		return (data & 0x2f) * (1 << ((data >> 10) & 1)) *
682 			(1 << ((data >> 9) & 1));
683 	return data;
684 }
685 
mt9m111_set_global_gain(struct mt9m111 * mt9m111,int gain)686 static int mt9m111_set_global_gain(struct mt9m111 *mt9m111, int gain)
687 {
688 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
689 	u16 val;
690 
691 	if (gain > 63 * 2 * 2)
692 		return -EINVAL;
693 
694 	if ((gain >= 64 * 2) && (gain < 63 * 2 * 2))
695 		val = (1 << 10) | (1 << 9) | (gain / 4);
696 	else if ((gain >= 64) && (gain < 64 * 2))
697 		val = (1 << 9) | (gain / 2);
698 	else
699 		val = gain;
700 
701 	return reg_write(GLOBAL_GAIN, val);
702 }
703 
mt9m111_set_autoexposure(struct mt9m111 * mt9m111,int on)704 static int mt9m111_set_autoexposure(struct mt9m111 *mt9m111, int on)
705 {
706 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
707 
708 	if (on)
709 		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
710 	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOEXPO_EN);
711 }
712 
mt9m111_set_autowhitebalance(struct mt9m111 * mt9m111,int on)713 static int mt9m111_set_autowhitebalance(struct mt9m111 *mt9m111, int on)
714 {
715 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
716 
717 	if (on)
718 		return reg_set(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
719 	return reg_clear(OPER_MODE_CTRL, MT9M111_OPMODE_AUTOWHITEBAL_EN);
720 }
721 
mt9m111_s_ctrl(struct v4l2_ctrl * ctrl)722 static int mt9m111_s_ctrl(struct v4l2_ctrl *ctrl)
723 {
724 	struct mt9m111 *mt9m111 = container_of(ctrl->handler,
725 					       struct mt9m111, hdl);
726 
727 	switch (ctrl->id) {
728 	case V4L2_CID_VFLIP:
729 		return mt9m111_set_flip(mt9m111, ctrl->val,
730 					MT9M111_RMB_MIRROR_ROWS);
731 	case V4L2_CID_HFLIP:
732 		return mt9m111_set_flip(mt9m111, ctrl->val,
733 					MT9M111_RMB_MIRROR_COLS);
734 	case V4L2_CID_GAIN:
735 		return mt9m111_set_global_gain(mt9m111, ctrl->val);
736 	case V4L2_CID_EXPOSURE_AUTO:
737 		return mt9m111_set_autoexposure(mt9m111, ctrl->val);
738 	case V4L2_CID_AUTO_WHITE_BALANCE:
739 		return mt9m111_set_autowhitebalance(mt9m111, ctrl->val);
740 	}
741 
742 	return -EINVAL;
743 }
744 
mt9m111_suspend(struct mt9m111 * mt9m111)745 static int mt9m111_suspend(struct mt9m111 *mt9m111)
746 {
747 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
748 	int ret;
749 
750 	v4l2_ctrl_s_ctrl(mt9m111->gain, mt9m111_get_global_gain(mt9m111));
751 
752 	ret = reg_set(RESET, MT9M111_RESET_RESET_MODE);
753 	if (!ret)
754 		ret = reg_set(RESET, MT9M111_RESET_RESET_SOC |
755 			      MT9M111_RESET_OUTPUT_DISABLE |
756 			      MT9M111_RESET_ANALOG_STANDBY);
757 	if (!ret)
758 		ret = reg_clear(RESET, MT9M111_RESET_CHIP_ENABLE);
759 
760 	return ret;
761 }
762 
mt9m111_restore_state(struct mt9m111 * mt9m111)763 static void mt9m111_restore_state(struct mt9m111 *mt9m111)
764 {
765 	mt9m111_set_context(mt9m111, mt9m111->ctx);
766 	mt9m111_set_pixfmt(mt9m111, mt9m111->fmt->code);
767 	mt9m111_setup_geometry(mt9m111, &mt9m111->rect,
768 			mt9m111->width, mt9m111->height, mt9m111->fmt->code);
769 	v4l2_ctrl_handler_setup(&mt9m111->hdl);
770 }
771 
mt9m111_resume(struct mt9m111 * mt9m111)772 static int mt9m111_resume(struct mt9m111 *mt9m111)
773 {
774 	int ret = mt9m111_enable(mt9m111);
775 	if (!ret)
776 		ret = mt9m111_reset(mt9m111);
777 	if (!ret)
778 		mt9m111_restore_state(mt9m111);
779 
780 	return ret;
781 }
782 
mt9m111_init(struct mt9m111 * mt9m111)783 static int mt9m111_init(struct mt9m111 *mt9m111)
784 {
785 	struct i2c_client *client = v4l2_get_subdevdata(&mt9m111->subdev);
786 	int ret;
787 
788 	/* Default HIGHPOWER context */
789 	mt9m111->ctx = &context_b;
790 	ret = mt9m111_enable(mt9m111);
791 	if (!ret)
792 		ret = mt9m111_reset(mt9m111);
793 	if (!ret)
794 		ret = mt9m111_set_context(mt9m111, mt9m111->ctx);
795 	if (ret)
796 		dev_err(&client->dev, "mt9m111 init failed: %d\n", ret);
797 	return ret;
798 }
799 
800 /*
801  * Interface active, can use i2c. If it fails, it can indeed mean, that
802  * this wasn't our capture interface, so, we wait for the right one
803  */
mt9m111_video_probe(struct i2c_client * client)804 static int mt9m111_video_probe(struct i2c_client *client)
805 {
806 	struct mt9m111 *mt9m111 = to_mt9m111(client);
807 	s32 data;
808 	int ret;
809 
810 	data = reg_read(CHIP_VERSION);
811 
812 	switch (data) {
813 	case 0x143a: /* MT9M111 or MT9M131 */
814 		mt9m111->model = V4L2_IDENT_MT9M111;
815 		dev_info(&client->dev,
816 			"Detected a MT9M111/MT9M131 chip ID %x\n", data);
817 		break;
818 	case 0x148c: /* MT9M112 */
819 		mt9m111->model = V4L2_IDENT_MT9M112;
820 		dev_info(&client->dev, "Detected a MT9M112 chip ID %x\n", data);
821 		break;
822 	default:
823 		dev_err(&client->dev,
824 			"No MT9M111/MT9M112/MT9M131 chip detected register read %x\n",
825 			data);
826 		return -ENODEV;
827 	}
828 
829 	ret = mt9m111_init(mt9m111);
830 	if (ret)
831 		return ret;
832 	return v4l2_ctrl_handler_setup(&mt9m111->hdl);
833 }
834 
mt9m111_s_power(struct v4l2_subdev * sd,int on)835 static int mt9m111_s_power(struct v4l2_subdev *sd, int on)
836 {
837 	struct mt9m111 *mt9m111 = container_of(sd, struct mt9m111, subdev);
838 	struct i2c_client *client = v4l2_get_subdevdata(sd);
839 	int ret = 0;
840 
841 	mutex_lock(&mt9m111->power_lock);
842 
843 	/*
844 	 * If the power count is modified from 0 to != 0 or from != 0 to 0,
845 	 * update the power state.
846 	 */
847 	if (mt9m111->power_count == !on) {
848 		if (on) {
849 			ret = mt9m111_resume(mt9m111);
850 			if (ret) {
851 				dev_err(&client->dev,
852 					"Failed to resume the sensor: %d\n", ret);
853 				goto out;
854 			}
855 		} else {
856 			mt9m111_suspend(mt9m111);
857 		}
858 	}
859 
860 	/* Update the power count. */
861 	mt9m111->power_count += on ? 1 : -1;
862 	WARN_ON(mt9m111->power_count < 0);
863 
864 out:
865 	mutex_unlock(&mt9m111->power_lock);
866 	return ret;
867 }
868 
869 static const struct v4l2_ctrl_ops mt9m111_ctrl_ops = {
870 	.s_ctrl = mt9m111_s_ctrl,
871 };
872 
873 static struct v4l2_subdev_core_ops mt9m111_subdev_core_ops = {
874 	.g_chip_ident	= mt9m111_g_chip_ident,
875 	.s_power	= mt9m111_s_power,
876 #ifdef CONFIG_VIDEO_ADV_DEBUG
877 	.g_register	= mt9m111_g_register,
878 	.s_register	= mt9m111_s_register,
879 #endif
880 };
881 
mt9m111_enum_fmt(struct v4l2_subdev * sd,unsigned int index,enum v4l2_mbus_pixelcode * code)882 static int mt9m111_enum_fmt(struct v4l2_subdev *sd, unsigned int index,
883 			    enum v4l2_mbus_pixelcode *code)
884 {
885 	if (index >= ARRAY_SIZE(mt9m111_colour_fmts))
886 		return -EINVAL;
887 
888 	*code = mt9m111_colour_fmts[index].code;
889 	return 0;
890 }
891 
mt9m111_g_mbus_config(struct v4l2_subdev * sd,struct v4l2_mbus_config * cfg)892 static int mt9m111_g_mbus_config(struct v4l2_subdev *sd,
893 				struct v4l2_mbus_config *cfg)
894 {
895 	struct i2c_client *client = v4l2_get_subdevdata(sd);
896 	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
897 
898 	cfg->flags = V4L2_MBUS_MASTER | V4L2_MBUS_PCLK_SAMPLE_RISING |
899 		V4L2_MBUS_HSYNC_ACTIVE_HIGH | V4L2_MBUS_VSYNC_ACTIVE_HIGH |
900 		V4L2_MBUS_DATA_ACTIVE_HIGH;
901 	cfg->type = V4L2_MBUS_PARALLEL;
902 	cfg->flags = soc_camera_apply_board_flags(icl, cfg);
903 
904 	return 0;
905 }
906 
907 static struct v4l2_subdev_video_ops mt9m111_subdev_video_ops = {
908 	.s_mbus_fmt	= mt9m111_s_fmt,
909 	.g_mbus_fmt	= mt9m111_g_fmt,
910 	.try_mbus_fmt	= mt9m111_try_fmt,
911 	.s_crop		= mt9m111_s_crop,
912 	.g_crop		= mt9m111_g_crop,
913 	.cropcap	= mt9m111_cropcap,
914 	.enum_mbus_fmt	= mt9m111_enum_fmt,
915 	.g_mbus_config	= mt9m111_g_mbus_config,
916 };
917 
918 static struct v4l2_subdev_ops mt9m111_subdev_ops = {
919 	.core	= &mt9m111_subdev_core_ops,
920 	.video	= &mt9m111_subdev_video_ops,
921 };
922 
mt9m111_probe(struct i2c_client * client,const struct i2c_device_id * did)923 static int mt9m111_probe(struct i2c_client *client,
924 			 const struct i2c_device_id *did)
925 {
926 	struct mt9m111 *mt9m111;
927 	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
928 	struct soc_camera_link *icl = soc_camera_i2c_to_link(client);
929 	int ret;
930 
931 	if (!icl) {
932 		dev_err(&client->dev, "mt9m111: driver needs platform data\n");
933 		return -EINVAL;
934 	}
935 
936 	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
937 		dev_warn(&adapter->dev,
938 			 "I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
939 		return -EIO;
940 	}
941 
942 	mt9m111 = kzalloc(sizeof(struct mt9m111), GFP_KERNEL);
943 	if (!mt9m111)
944 		return -ENOMEM;
945 
946 	v4l2_i2c_subdev_init(&mt9m111->subdev, client, &mt9m111_subdev_ops);
947 	v4l2_ctrl_handler_init(&mt9m111->hdl, 5);
948 	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
949 			V4L2_CID_VFLIP, 0, 1, 1, 0);
950 	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
951 			V4L2_CID_HFLIP, 0, 1, 1, 0);
952 	v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
953 			V4L2_CID_AUTO_WHITE_BALANCE, 0, 1, 1, 1);
954 	mt9m111->gain = v4l2_ctrl_new_std(&mt9m111->hdl, &mt9m111_ctrl_ops,
955 			V4L2_CID_GAIN, 0, 63 * 2 * 2, 1, 32);
956 	v4l2_ctrl_new_std_menu(&mt9m111->hdl,
957 			&mt9m111_ctrl_ops, V4L2_CID_EXPOSURE_AUTO, 1, 0,
958 			V4L2_EXPOSURE_AUTO);
959 	mt9m111->subdev.ctrl_handler = &mt9m111->hdl;
960 	if (mt9m111->hdl.error) {
961 		int err = mt9m111->hdl.error;
962 
963 		kfree(mt9m111);
964 		return err;
965 	}
966 
967 	/* Second stage probe - when a capture adapter is there */
968 	mt9m111->rect.left	= MT9M111_MIN_DARK_COLS;
969 	mt9m111->rect.top	= MT9M111_MIN_DARK_ROWS;
970 	mt9m111->rect.width	= MT9M111_MAX_WIDTH;
971 	mt9m111->rect.height	= MT9M111_MAX_HEIGHT;
972 	mt9m111->fmt		= &mt9m111_colour_fmts[0];
973 	mt9m111->lastpage	= -1;
974 	mutex_init(&mt9m111->power_lock);
975 
976 	ret = mt9m111_video_probe(client);
977 	if (ret) {
978 		v4l2_ctrl_handler_free(&mt9m111->hdl);
979 		kfree(mt9m111);
980 	}
981 
982 	return ret;
983 }
984 
mt9m111_remove(struct i2c_client * client)985 static int mt9m111_remove(struct i2c_client *client)
986 {
987 	struct mt9m111 *mt9m111 = to_mt9m111(client);
988 
989 	v4l2_device_unregister_subdev(&mt9m111->subdev);
990 	v4l2_ctrl_handler_free(&mt9m111->hdl);
991 	kfree(mt9m111);
992 
993 	return 0;
994 }
995 
996 static const struct i2c_device_id mt9m111_id[] = {
997 	{ "mt9m111", 0 },
998 	{ }
999 };
1000 MODULE_DEVICE_TABLE(i2c, mt9m111_id);
1001 
1002 static struct i2c_driver mt9m111_i2c_driver = {
1003 	.driver = {
1004 		.name = "mt9m111",
1005 	},
1006 	.probe		= mt9m111_probe,
1007 	.remove		= mt9m111_remove,
1008 	.id_table	= mt9m111_id,
1009 };
1010 
mt9m111_mod_init(void)1011 static int __init mt9m111_mod_init(void)
1012 {
1013 	return i2c_add_driver(&mt9m111_i2c_driver);
1014 }
1015 
mt9m111_mod_exit(void)1016 static void __exit mt9m111_mod_exit(void)
1017 {
1018 	i2c_del_driver(&mt9m111_i2c_driver);
1019 }
1020 
1021 module_init(mt9m111_mod_init);
1022 module_exit(mt9m111_mod_exit);
1023 
1024 MODULE_DESCRIPTION("Micron/Aptina MT9M111/MT9M112/MT9M131 Camera driver");
1025 MODULE_AUTHOR("Robert Jarzmik");
1026 MODULE_LICENSE("GPL");
1027