[go: nahoru, domu]

1/*
2 * Driver for MT9P031 CMOS Image Sensor from Aptina
3 *
4 * Copyright (C) 2011, Laurent Pinchart <laurent.pinchart@ideasonboard.com>
5 * Copyright (C) 2011, Javier Martin <javier.martin@vista-silicon.com>
6 * Copyright (C) 2011, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
7 *
8 * Based on the MT9V032 driver and Bastian Hecht's code.
9 *
10 * This program is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License version 2 as
12 * published by the Free Software Foundation.
13 */
14
15#include <linux/clk.h>
16#include <linux/delay.h>
17#include <linux/device.h>
18#include <linux/gpio.h>
19#include <linux/i2c.h>
20#include <linux/log2.h>
21#include <linux/module.h>
22#include <linux/of.h>
23#include <linux/of_gpio.h>
24#include <linux/of_graph.h>
25#include <linux/pm.h>
26#include <linux/regulator/consumer.h>
27#include <linux/slab.h>
28#include <linux/videodev2.h>
29
30#include <media/mt9p031.h>
31#include <media/v4l2-ctrls.h>
32#include <media/v4l2-device.h>
33#include <media/v4l2-subdev.h>
34
35#include "aptina-pll.h"
36
37#define MT9P031_PIXEL_ARRAY_WIDTH			2752
38#define MT9P031_PIXEL_ARRAY_HEIGHT			2004
39
40#define MT9P031_CHIP_VERSION				0x00
41#define		MT9P031_CHIP_VERSION_VALUE		0x1801
42#define MT9P031_ROW_START				0x01
43#define		MT9P031_ROW_START_MIN			0
44#define		MT9P031_ROW_START_MAX			2004
45#define		MT9P031_ROW_START_DEF			54
46#define MT9P031_COLUMN_START				0x02
47#define		MT9P031_COLUMN_START_MIN		0
48#define		MT9P031_COLUMN_START_MAX		2750
49#define		MT9P031_COLUMN_START_DEF		16
50#define MT9P031_WINDOW_HEIGHT				0x03
51#define		MT9P031_WINDOW_HEIGHT_MIN		2
52#define		MT9P031_WINDOW_HEIGHT_MAX		2006
53#define		MT9P031_WINDOW_HEIGHT_DEF		1944
54#define MT9P031_WINDOW_WIDTH				0x04
55#define		MT9P031_WINDOW_WIDTH_MIN		2
56#define		MT9P031_WINDOW_WIDTH_MAX		2752
57#define		MT9P031_WINDOW_WIDTH_DEF		2592
58#define MT9P031_HORIZONTAL_BLANK			0x05
59#define		MT9P031_HORIZONTAL_BLANK_MIN		0
60#define		MT9P031_HORIZONTAL_BLANK_MAX		4095
61#define MT9P031_VERTICAL_BLANK				0x06
62#define		MT9P031_VERTICAL_BLANK_MIN		1
63#define		MT9P031_VERTICAL_BLANK_MAX		4096
64#define		MT9P031_VERTICAL_BLANK_DEF		26
65#define MT9P031_OUTPUT_CONTROL				0x07
66#define		MT9P031_OUTPUT_CONTROL_CEN		2
67#define		MT9P031_OUTPUT_CONTROL_SYN		1
68#define		MT9P031_OUTPUT_CONTROL_DEF		0x1f82
69#define MT9P031_SHUTTER_WIDTH_UPPER			0x08
70#define MT9P031_SHUTTER_WIDTH_LOWER			0x09
71#define		MT9P031_SHUTTER_WIDTH_MIN		1
72#define		MT9P031_SHUTTER_WIDTH_MAX		1048575
73#define		MT9P031_SHUTTER_WIDTH_DEF		1943
74#define	MT9P031_PLL_CONTROL				0x10
75#define		MT9P031_PLL_CONTROL_PWROFF		0x0050
76#define		MT9P031_PLL_CONTROL_PWRON		0x0051
77#define		MT9P031_PLL_CONTROL_USEPLL		0x0052
78#define	MT9P031_PLL_CONFIG_1				0x11
79#define	MT9P031_PLL_CONFIG_2				0x12
80#define MT9P031_PIXEL_CLOCK_CONTROL			0x0a
81#define		MT9P031_PIXEL_CLOCK_INVERT		(1 << 15)
82#define		MT9P031_PIXEL_CLOCK_SHIFT(n)		((n) << 8)
83#define		MT9P031_PIXEL_CLOCK_DIVIDE(n)		((n) << 0)
84#define MT9P031_FRAME_RESTART				0x0b
85#define MT9P031_SHUTTER_DELAY				0x0c
86#define MT9P031_RST					0x0d
87#define		MT9P031_RST_ENABLE			1
88#define		MT9P031_RST_DISABLE			0
89#define MT9P031_READ_MODE_1				0x1e
90#define MT9P031_READ_MODE_2				0x20
91#define		MT9P031_READ_MODE_2_ROW_MIR		(1 << 15)
92#define		MT9P031_READ_MODE_2_COL_MIR		(1 << 14)
93#define		MT9P031_READ_MODE_2_ROW_BLC		(1 << 6)
94#define MT9P031_ROW_ADDRESS_MODE			0x22
95#define MT9P031_COLUMN_ADDRESS_MODE			0x23
96#define MT9P031_GLOBAL_GAIN				0x35
97#define		MT9P031_GLOBAL_GAIN_MIN			8
98#define		MT9P031_GLOBAL_GAIN_MAX			1024
99#define		MT9P031_GLOBAL_GAIN_DEF			8
100#define		MT9P031_GLOBAL_GAIN_MULT		(1 << 6)
101#define MT9P031_ROW_BLACK_TARGET			0x49
102#define MT9P031_ROW_BLACK_DEF_OFFSET			0x4b
103#define MT9P031_GREEN1_OFFSET				0x60
104#define MT9P031_GREEN2_OFFSET				0x61
105#define MT9P031_BLACK_LEVEL_CALIBRATION			0x62
106#define		MT9P031_BLC_MANUAL_BLC			(1 << 0)
107#define MT9P031_RED_OFFSET				0x63
108#define MT9P031_BLUE_OFFSET				0x64
109#define MT9P031_TEST_PATTERN				0xa0
110#define		MT9P031_TEST_PATTERN_SHIFT		3
111#define		MT9P031_TEST_PATTERN_ENABLE		(1 << 0)
112#define		MT9P031_TEST_PATTERN_DISABLE		(0 << 0)
113#define MT9P031_TEST_PATTERN_GREEN			0xa1
114#define MT9P031_TEST_PATTERN_RED			0xa2
115#define MT9P031_TEST_PATTERN_BLUE			0xa3
116
117enum mt9p031_model {
118	MT9P031_MODEL_COLOR,
119	MT9P031_MODEL_MONOCHROME,
120};
121
122struct mt9p031 {
123	struct v4l2_subdev subdev;
124	struct media_pad pad;
125	struct v4l2_rect crop;  /* Sensor window */
126	struct v4l2_mbus_framefmt format;
127	struct mt9p031_platform_data *pdata;
128	struct mutex power_lock; /* lock to protect power_count */
129	int power_count;
130
131	struct clk *clk;
132	struct regulator_bulk_data regulators[3];
133
134	enum mt9p031_model model;
135	struct aptina_pll pll;
136	unsigned int clk_div;
137	bool use_pll;
138	int reset;
139
140	struct v4l2_ctrl_handler ctrls;
141	struct v4l2_ctrl *blc_auto;
142	struct v4l2_ctrl *blc_offset;
143
144	/* Registers cache */
145	u16 output_control;
146	u16 mode2;
147};
148
149static struct mt9p031 *to_mt9p031(struct v4l2_subdev *sd)
150{
151	return container_of(sd, struct mt9p031, subdev);
152}
153
154static int mt9p031_read(struct i2c_client *client, u8 reg)
155{
156	return i2c_smbus_read_word_swapped(client, reg);
157}
158
159static int mt9p031_write(struct i2c_client *client, u8 reg, u16 data)
160{
161	return i2c_smbus_write_word_swapped(client, reg, data);
162}
163
164static int mt9p031_set_output_control(struct mt9p031 *mt9p031, u16 clear,
165				      u16 set)
166{
167	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
168	u16 value = (mt9p031->output_control & ~clear) | set;
169	int ret;
170
171	ret = mt9p031_write(client, MT9P031_OUTPUT_CONTROL, value);
172	if (ret < 0)
173		return ret;
174
175	mt9p031->output_control = value;
176	return 0;
177}
178
179static int mt9p031_set_mode2(struct mt9p031 *mt9p031, u16 clear, u16 set)
180{
181	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
182	u16 value = (mt9p031->mode2 & ~clear) | set;
183	int ret;
184
185	ret = mt9p031_write(client, MT9P031_READ_MODE_2, value);
186	if (ret < 0)
187		return ret;
188
189	mt9p031->mode2 = value;
190	return 0;
191}
192
193static int mt9p031_reset(struct mt9p031 *mt9p031)
194{
195	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
196	int ret;
197
198	/* Disable chip output, synchronous option update */
199	ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_ENABLE);
200	if (ret < 0)
201		return ret;
202	ret = mt9p031_write(client, MT9P031_RST, MT9P031_RST_DISABLE);
203	if (ret < 0)
204		return ret;
205
206	ret = mt9p031_write(client, MT9P031_PIXEL_CLOCK_CONTROL,
207			    MT9P031_PIXEL_CLOCK_DIVIDE(mt9p031->clk_div));
208	if (ret < 0)
209		return ret;
210
211	return mt9p031_set_output_control(mt9p031, MT9P031_OUTPUT_CONTROL_CEN,
212					  0);
213}
214
215static int mt9p031_clk_setup(struct mt9p031 *mt9p031)
216{
217	static const struct aptina_pll_limits limits = {
218		.ext_clock_min = 6000000,
219		.ext_clock_max = 27000000,
220		.int_clock_min = 2000000,
221		.int_clock_max = 13500000,
222		.out_clock_min = 180000000,
223		.out_clock_max = 360000000,
224		.pix_clock_max = 96000000,
225		.n_min = 1,
226		.n_max = 64,
227		.m_min = 16,
228		.m_max = 255,
229		.p1_min = 1,
230		.p1_max = 128,
231	};
232
233	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
234	struct mt9p031_platform_data *pdata = mt9p031->pdata;
235	int ret;
236
237	mt9p031->clk = devm_clk_get(&client->dev, NULL);
238	if (IS_ERR(mt9p031->clk))
239		return PTR_ERR(mt9p031->clk);
240
241	ret = clk_set_rate(mt9p031->clk, pdata->ext_freq);
242	if (ret < 0)
243		return ret;
244
245	/* If the external clock frequency is out of bounds for the PLL use the
246	 * pixel clock divider only and disable the PLL.
247	 */
248	if (pdata->ext_freq > limits.ext_clock_max) {
249		unsigned int div;
250
251		div = DIV_ROUND_UP(pdata->ext_freq, pdata->target_freq);
252		div = roundup_pow_of_two(div) / 2;
253
254		mt9p031->clk_div = max_t(unsigned int, div, 64);
255		mt9p031->use_pll = false;
256
257		return 0;
258	}
259
260	mt9p031->pll.ext_clock = pdata->ext_freq;
261	mt9p031->pll.pix_clock = pdata->target_freq;
262	mt9p031->use_pll = true;
263
264	return aptina_pll_calculate(&client->dev, &limits, &mt9p031->pll);
265}
266
267static int mt9p031_pll_enable(struct mt9p031 *mt9p031)
268{
269	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
270	int ret;
271
272	if (!mt9p031->use_pll)
273		return 0;
274
275	ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
276			    MT9P031_PLL_CONTROL_PWRON);
277	if (ret < 0)
278		return ret;
279
280	ret = mt9p031_write(client, MT9P031_PLL_CONFIG_1,
281			    (mt9p031->pll.m << 8) | (mt9p031->pll.n - 1));
282	if (ret < 0)
283		return ret;
284
285	ret = mt9p031_write(client, MT9P031_PLL_CONFIG_2, mt9p031->pll.p1 - 1);
286	if (ret < 0)
287		return ret;
288
289	usleep_range(1000, 2000);
290	ret = mt9p031_write(client, MT9P031_PLL_CONTROL,
291			    MT9P031_PLL_CONTROL_PWRON |
292			    MT9P031_PLL_CONTROL_USEPLL);
293	return ret;
294}
295
296static inline int mt9p031_pll_disable(struct mt9p031 *mt9p031)
297{
298	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
299
300	if (!mt9p031->use_pll)
301		return 0;
302
303	return mt9p031_write(client, MT9P031_PLL_CONTROL,
304			     MT9P031_PLL_CONTROL_PWROFF);
305}
306
307static int mt9p031_power_on(struct mt9p031 *mt9p031)
308{
309	int ret;
310
311	/* Ensure RESET_BAR is low */
312	if (gpio_is_valid(mt9p031->reset)) {
313		gpio_set_value(mt9p031->reset, 0);
314		usleep_range(1000, 2000);
315	}
316
317	/* Bring up the supplies */
318	ret = regulator_bulk_enable(ARRAY_SIZE(mt9p031->regulators),
319				   mt9p031->regulators);
320	if (ret < 0)
321		return ret;
322
323	/* Enable clock */
324	if (mt9p031->clk) {
325		ret = clk_prepare_enable(mt9p031->clk);
326		if (ret) {
327			regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
328					       mt9p031->regulators);
329			return ret;
330		}
331	}
332
333	/* Now RESET_BAR must be high */
334	if (gpio_is_valid(mt9p031->reset)) {
335		gpio_set_value(mt9p031->reset, 1);
336		usleep_range(1000, 2000);
337	}
338
339	return 0;
340}
341
342static void mt9p031_power_off(struct mt9p031 *mt9p031)
343{
344	if (gpio_is_valid(mt9p031->reset)) {
345		gpio_set_value(mt9p031->reset, 0);
346		usleep_range(1000, 2000);
347	}
348
349	regulator_bulk_disable(ARRAY_SIZE(mt9p031->regulators),
350			       mt9p031->regulators);
351
352	if (mt9p031->clk)
353		clk_disable_unprepare(mt9p031->clk);
354}
355
356static int __mt9p031_set_power(struct mt9p031 *mt9p031, bool on)
357{
358	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
359	int ret;
360
361	if (!on) {
362		mt9p031_power_off(mt9p031);
363		return 0;
364	}
365
366	ret = mt9p031_power_on(mt9p031);
367	if (ret < 0)
368		return ret;
369
370	ret = mt9p031_reset(mt9p031);
371	if (ret < 0) {
372		dev_err(&client->dev, "Failed to reset the camera\n");
373		return ret;
374	}
375
376	return v4l2_ctrl_handler_setup(&mt9p031->ctrls);
377}
378
379/* -----------------------------------------------------------------------------
380 * V4L2 subdev video operations
381 */
382
383static int mt9p031_set_params(struct mt9p031 *mt9p031)
384{
385	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
386	struct v4l2_mbus_framefmt *format = &mt9p031->format;
387	const struct v4l2_rect *crop = &mt9p031->crop;
388	unsigned int hblank;
389	unsigned int vblank;
390	unsigned int xskip;
391	unsigned int yskip;
392	unsigned int xbin;
393	unsigned int ybin;
394	int ret;
395
396	/* Windows position and size.
397	 *
398	 * TODO: Make sure the start coordinates and window size match the
399	 * skipping, binning and mirroring (see description of registers 2 and 4
400	 * in table 13, and Binning section on page 41).
401	 */
402	ret = mt9p031_write(client, MT9P031_COLUMN_START, crop->left);
403	if (ret < 0)
404		return ret;
405	ret = mt9p031_write(client, MT9P031_ROW_START, crop->top);
406	if (ret < 0)
407		return ret;
408	ret = mt9p031_write(client, MT9P031_WINDOW_WIDTH, crop->width - 1);
409	if (ret < 0)
410		return ret;
411	ret = mt9p031_write(client, MT9P031_WINDOW_HEIGHT, crop->height - 1);
412	if (ret < 0)
413		return ret;
414
415	/* Row and column binning and skipping. Use the maximum binning value
416	 * compatible with the skipping settings.
417	 */
418	xskip = DIV_ROUND_CLOSEST(crop->width, format->width);
419	yskip = DIV_ROUND_CLOSEST(crop->height, format->height);
420	xbin = 1 << (ffs(xskip) - 1);
421	ybin = 1 << (ffs(yskip) - 1);
422
423	ret = mt9p031_write(client, MT9P031_COLUMN_ADDRESS_MODE,
424			    ((xbin - 1) << 4) | (xskip - 1));
425	if (ret < 0)
426		return ret;
427	ret = mt9p031_write(client, MT9P031_ROW_ADDRESS_MODE,
428			    ((ybin - 1) << 4) | (yskip - 1));
429	if (ret < 0)
430		return ret;
431
432	/* Blanking - use minimum value for horizontal blanking and default
433	 * value for vertical blanking.
434	 */
435	hblank = 346 * ybin + 64 + (80 >> min_t(unsigned int, xbin, 3));
436	vblank = MT9P031_VERTICAL_BLANK_DEF;
437
438	ret = mt9p031_write(client, MT9P031_HORIZONTAL_BLANK, hblank - 1);
439	if (ret < 0)
440		return ret;
441	ret = mt9p031_write(client, MT9P031_VERTICAL_BLANK, vblank - 1);
442	if (ret < 0)
443		return ret;
444
445	return ret;
446}
447
448static int mt9p031_s_stream(struct v4l2_subdev *subdev, int enable)
449{
450	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
451	int ret;
452
453	if (!enable) {
454		/* Stop sensor readout */
455		ret = mt9p031_set_output_control(mt9p031,
456						 MT9P031_OUTPUT_CONTROL_CEN, 0);
457		if (ret < 0)
458			return ret;
459
460		return mt9p031_pll_disable(mt9p031);
461	}
462
463	ret = mt9p031_set_params(mt9p031);
464	if (ret < 0)
465		return ret;
466
467	/* Switch to master "normal" mode */
468	ret = mt9p031_set_output_control(mt9p031, 0,
469					 MT9P031_OUTPUT_CONTROL_CEN);
470	if (ret < 0)
471		return ret;
472
473	return mt9p031_pll_enable(mt9p031);
474}
475
476static int mt9p031_enum_mbus_code(struct v4l2_subdev *subdev,
477				  struct v4l2_subdev_fh *fh,
478				  struct v4l2_subdev_mbus_code_enum *code)
479{
480	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
481
482	if (code->pad || code->index)
483		return -EINVAL;
484
485	code->code = mt9p031->format.code;
486	return 0;
487}
488
489static int mt9p031_enum_frame_size(struct v4l2_subdev *subdev,
490				   struct v4l2_subdev_fh *fh,
491				   struct v4l2_subdev_frame_size_enum *fse)
492{
493	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
494
495	if (fse->index >= 8 || fse->code != mt9p031->format.code)
496		return -EINVAL;
497
498	fse->min_width = MT9P031_WINDOW_WIDTH_DEF
499		       / min_t(unsigned int, 7, fse->index + 1);
500	fse->max_width = fse->min_width;
501	fse->min_height = MT9P031_WINDOW_HEIGHT_DEF / (fse->index + 1);
502	fse->max_height = fse->min_height;
503
504	return 0;
505}
506
507static struct v4l2_mbus_framefmt *
508__mt9p031_get_pad_format(struct mt9p031 *mt9p031, struct v4l2_subdev_fh *fh,
509			 unsigned int pad, u32 which)
510{
511	switch (which) {
512	case V4L2_SUBDEV_FORMAT_TRY:
513		return v4l2_subdev_get_try_format(fh, pad);
514	case V4L2_SUBDEV_FORMAT_ACTIVE:
515		return &mt9p031->format;
516	default:
517		return NULL;
518	}
519}
520
521static struct v4l2_rect *
522__mt9p031_get_pad_crop(struct mt9p031 *mt9p031, struct v4l2_subdev_fh *fh,
523		     unsigned int pad, u32 which)
524{
525	switch (which) {
526	case V4L2_SUBDEV_FORMAT_TRY:
527		return v4l2_subdev_get_try_crop(fh, pad);
528	case V4L2_SUBDEV_FORMAT_ACTIVE:
529		return &mt9p031->crop;
530	default:
531		return NULL;
532	}
533}
534
535static int mt9p031_get_format(struct v4l2_subdev *subdev,
536			      struct v4l2_subdev_fh *fh,
537			      struct v4l2_subdev_format *fmt)
538{
539	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
540
541	fmt->format = *__mt9p031_get_pad_format(mt9p031, fh, fmt->pad,
542						fmt->which);
543	return 0;
544}
545
546static int mt9p031_set_format(struct v4l2_subdev *subdev,
547			      struct v4l2_subdev_fh *fh,
548			      struct v4l2_subdev_format *format)
549{
550	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
551	struct v4l2_mbus_framefmt *__format;
552	struct v4l2_rect *__crop;
553	unsigned int width;
554	unsigned int height;
555	unsigned int hratio;
556	unsigned int vratio;
557
558	__crop = __mt9p031_get_pad_crop(mt9p031, fh, format->pad,
559					format->which);
560
561	/* Clamp the width and height to avoid dividing by zero. */
562	width = clamp_t(unsigned int, ALIGN(format->format.width, 2),
563			max_t(unsigned int, __crop->width / 7,
564			      MT9P031_WINDOW_WIDTH_MIN),
565			__crop->width);
566	height = clamp_t(unsigned int, ALIGN(format->format.height, 2),
567			 max_t(unsigned int, __crop->height / 8,
568			       MT9P031_WINDOW_HEIGHT_MIN),
569			 __crop->height);
570
571	hratio = DIV_ROUND_CLOSEST(__crop->width, width);
572	vratio = DIV_ROUND_CLOSEST(__crop->height, height);
573
574	__format = __mt9p031_get_pad_format(mt9p031, fh, format->pad,
575					    format->which);
576	__format->width = __crop->width / hratio;
577	__format->height = __crop->height / vratio;
578
579	format->format = *__format;
580
581	return 0;
582}
583
584static int mt9p031_get_crop(struct v4l2_subdev *subdev,
585			    struct v4l2_subdev_fh *fh,
586			    struct v4l2_subdev_crop *crop)
587{
588	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
589
590	crop->rect = *__mt9p031_get_pad_crop(mt9p031, fh, crop->pad,
591					     crop->which);
592	return 0;
593}
594
595static int mt9p031_set_crop(struct v4l2_subdev *subdev,
596			    struct v4l2_subdev_fh *fh,
597			    struct v4l2_subdev_crop *crop)
598{
599	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
600	struct v4l2_mbus_framefmt *__format;
601	struct v4l2_rect *__crop;
602	struct v4l2_rect rect;
603
604	/* Clamp the crop rectangle boundaries and align them to a multiple of 2
605	 * pixels to ensure a GRBG Bayer pattern.
606	 */
607	rect.left = clamp(ALIGN(crop->rect.left, 2), MT9P031_COLUMN_START_MIN,
608			  MT9P031_COLUMN_START_MAX);
609	rect.top = clamp(ALIGN(crop->rect.top, 2), MT9P031_ROW_START_MIN,
610			 MT9P031_ROW_START_MAX);
611	rect.width = clamp_t(unsigned int, ALIGN(crop->rect.width, 2),
612			     MT9P031_WINDOW_WIDTH_MIN,
613			     MT9P031_WINDOW_WIDTH_MAX);
614	rect.height = clamp_t(unsigned int, ALIGN(crop->rect.height, 2),
615			      MT9P031_WINDOW_HEIGHT_MIN,
616			      MT9P031_WINDOW_HEIGHT_MAX);
617
618	rect.width = min_t(unsigned int, rect.width,
619			   MT9P031_PIXEL_ARRAY_WIDTH - rect.left);
620	rect.height = min_t(unsigned int, rect.height,
621			    MT9P031_PIXEL_ARRAY_HEIGHT - rect.top);
622
623	__crop = __mt9p031_get_pad_crop(mt9p031, fh, crop->pad, crop->which);
624
625	if (rect.width != __crop->width || rect.height != __crop->height) {
626		/* Reset the output image size if the crop rectangle size has
627		 * been modified.
628		 */
629		__format = __mt9p031_get_pad_format(mt9p031, fh, crop->pad,
630						    crop->which);
631		__format->width = rect.width;
632		__format->height = rect.height;
633	}
634
635	*__crop = rect;
636	crop->rect = rect;
637
638	return 0;
639}
640
641/* -----------------------------------------------------------------------------
642 * V4L2 subdev control operations
643 */
644
645#define V4L2_CID_BLC_AUTO		(V4L2_CID_USER_BASE | 0x1002)
646#define V4L2_CID_BLC_TARGET_LEVEL	(V4L2_CID_USER_BASE | 0x1003)
647#define V4L2_CID_BLC_ANALOG_OFFSET	(V4L2_CID_USER_BASE | 0x1004)
648#define V4L2_CID_BLC_DIGITAL_OFFSET	(V4L2_CID_USER_BASE | 0x1005)
649
650static int mt9p031_restore_blc(struct mt9p031 *mt9p031)
651{
652	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
653	int ret;
654
655	if (mt9p031->blc_auto->cur.val != 0) {
656		ret = mt9p031_set_mode2(mt9p031, 0,
657					MT9P031_READ_MODE_2_ROW_BLC);
658		if (ret < 0)
659			return ret;
660	}
661
662	if (mt9p031->blc_offset->cur.val != 0) {
663		ret = mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
664				    mt9p031->blc_offset->cur.val);
665		if (ret < 0)
666			return ret;
667	}
668
669	return 0;
670}
671
672static int mt9p031_s_ctrl(struct v4l2_ctrl *ctrl)
673{
674	struct mt9p031 *mt9p031 =
675			container_of(ctrl->handler, struct mt9p031, ctrls);
676	struct i2c_client *client = v4l2_get_subdevdata(&mt9p031->subdev);
677	u16 data;
678	int ret;
679
680	if (ctrl->flags & V4L2_CTRL_FLAG_INACTIVE)
681		return 0;
682
683	switch (ctrl->id) {
684	case V4L2_CID_EXPOSURE:
685		ret = mt9p031_write(client, MT9P031_SHUTTER_WIDTH_UPPER,
686				    (ctrl->val >> 16) & 0xffff);
687		if (ret < 0)
688			return ret;
689
690		return mt9p031_write(client, MT9P031_SHUTTER_WIDTH_LOWER,
691				     ctrl->val & 0xffff);
692
693	case V4L2_CID_GAIN:
694		/* Gain is controlled by 2 analog stages and a digital stage.
695		 * Valid values for the 3 stages are
696		 *
697		 * Stage                Min     Max     Step
698		 * ------------------------------------------
699		 * First analog stage   x1      x2      1
700		 * Second analog stage  x1      x4      0.125
701		 * Digital stage        x1      x16     0.125
702		 *
703		 * To minimize noise, the gain stages should be used in the
704		 * second analog stage, first analog stage, digital stage order.
705		 * Gain from a previous stage should be pushed to its maximum
706		 * value before the next stage is used.
707		 */
708		if (ctrl->val <= 32) {
709			data = ctrl->val;
710		} else if (ctrl->val <= 64) {
711			ctrl->val &= ~1;
712			data = (1 << 6) | (ctrl->val >> 1);
713		} else {
714			ctrl->val &= ~7;
715			data = ((ctrl->val - 64) << 5) | (1 << 6) | 32;
716		}
717
718		return mt9p031_write(client, MT9P031_GLOBAL_GAIN, data);
719
720	case V4L2_CID_HFLIP:
721		if (ctrl->val)
722			return mt9p031_set_mode2(mt9p031,
723					0, MT9P031_READ_MODE_2_COL_MIR);
724		else
725			return mt9p031_set_mode2(mt9p031,
726					MT9P031_READ_MODE_2_COL_MIR, 0);
727
728	case V4L2_CID_VFLIP:
729		if (ctrl->val)
730			return mt9p031_set_mode2(mt9p031,
731					0, MT9P031_READ_MODE_2_ROW_MIR);
732		else
733			return mt9p031_set_mode2(mt9p031,
734					MT9P031_READ_MODE_2_ROW_MIR, 0);
735
736	case V4L2_CID_TEST_PATTERN:
737		/* The digital side of the Black Level Calibration function must
738		 * be disabled when generating a test pattern to avoid artifacts
739		 * in the image. Activate (deactivate) the BLC-related controls
740		 * when the test pattern is enabled (disabled).
741		 */
742		v4l2_ctrl_activate(mt9p031->blc_auto, ctrl->val == 0);
743		v4l2_ctrl_activate(mt9p031->blc_offset, ctrl->val == 0);
744
745		if (!ctrl->val) {
746			/* Restore the BLC settings. */
747			ret = mt9p031_restore_blc(mt9p031);
748			if (ret < 0)
749				return ret;
750
751			return mt9p031_write(client, MT9P031_TEST_PATTERN,
752					     MT9P031_TEST_PATTERN_DISABLE);
753		}
754
755		ret = mt9p031_write(client, MT9P031_TEST_PATTERN_GREEN, 0x05a0);
756		if (ret < 0)
757			return ret;
758		ret = mt9p031_write(client, MT9P031_TEST_PATTERN_RED, 0x0a50);
759		if (ret < 0)
760			return ret;
761		ret = mt9p031_write(client, MT9P031_TEST_PATTERN_BLUE, 0x0aa0);
762		if (ret < 0)
763			return ret;
764
765		/* Disable digital BLC when generating a test pattern. */
766		ret = mt9p031_set_mode2(mt9p031, MT9P031_READ_MODE_2_ROW_BLC,
767					0);
768		if (ret < 0)
769			return ret;
770
771		ret = mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET, 0);
772		if (ret < 0)
773			return ret;
774
775		return mt9p031_write(client, MT9P031_TEST_PATTERN,
776				((ctrl->val - 1) << MT9P031_TEST_PATTERN_SHIFT)
777				| MT9P031_TEST_PATTERN_ENABLE);
778
779	case V4L2_CID_BLC_AUTO:
780		ret = mt9p031_set_mode2(mt9p031,
781				ctrl->val ? 0 : MT9P031_READ_MODE_2_ROW_BLC,
782				ctrl->val ? MT9P031_READ_MODE_2_ROW_BLC : 0);
783		if (ret < 0)
784			return ret;
785
786		return mt9p031_write(client, MT9P031_BLACK_LEVEL_CALIBRATION,
787				     ctrl->val ? 0 : MT9P031_BLC_MANUAL_BLC);
788
789	case V4L2_CID_BLC_TARGET_LEVEL:
790		return mt9p031_write(client, MT9P031_ROW_BLACK_TARGET,
791				     ctrl->val);
792
793	case V4L2_CID_BLC_ANALOG_OFFSET:
794		data = ctrl->val & ((1 << 9) - 1);
795
796		ret = mt9p031_write(client, MT9P031_GREEN1_OFFSET, data);
797		if (ret < 0)
798			return ret;
799		ret = mt9p031_write(client, MT9P031_GREEN2_OFFSET, data);
800		if (ret < 0)
801			return ret;
802		ret = mt9p031_write(client, MT9P031_RED_OFFSET, data);
803		if (ret < 0)
804			return ret;
805		return mt9p031_write(client, MT9P031_BLUE_OFFSET, data);
806
807	case V4L2_CID_BLC_DIGITAL_OFFSET:
808		return mt9p031_write(client, MT9P031_ROW_BLACK_DEF_OFFSET,
809				     ctrl->val & ((1 << 12) - 1));
810	}
811
812	return 0;
813}
814
815static struct v4l2_ctrl_ops mt9p031_ctrl_ops = {
816	.s_ctrl = mt9p031_s_ctrl,
817};
818
819static const char * const mt9p031_test_pattern_menu[] = {
820	"Disabled",
821	"Color Field",
822	"Horizontal Gradient",
823	"Vertical Gradient",
824	"Diagonal Gradient",
825	"Classic Test Pattern",
826	"Walking 1s",
827	"Monochrome Horizontal Bars",
828	"Monochrome Vertical Bars",
829	"Vertical Color Bars",
830};
831
832static const struct v4l2_ctrl_config mt9p031_ctrls[] = {
833	{
834		.ops		= &mt9p031_ctrl_ops,
835		.id		= V4L2_CID_BLC_AUTO,
836		.type		= V4L2_CTRL_TYPE_BOOLEAN,
837		.name		= "BLC, Auto",
838		.min		= 0,
839		.max		= 1,
840		.step		= 1,
841		.def		= 1,
842		.flags		= 0,
843	}, {
844		.ops		= &mt9p031_ctrl_ops,
845		.id		= V4L2_CID_BLC_TARGET_LEVEL,
846		.type		= V4L2_CTRL_TYPE_INTEGER,
847		.name		= "BLC Target Level",
848		.min		= 0,
849		.max		= 4095,
850		.step		= 1,
851		.def		= 168,
852		.flags		= 0,
853	}, {
854		.ops		= &mt9p031_ctrl_ops,
855		.id		= V4L2_CID_BLC_ANALOG_OFFSET,
856		.type		= V4L2_CTRL_TYPE_INTEGER,
857		.name		= "BLC Analog Offset",
858		.min		= -255,
859		.max		= 255,
860		.step		= 1,
861		.def		= 32,
862		.flags		= 0,
863	}, {
864		.ops		= &mt9p031_ctrl_ops,
865		.id		= V4L2_CID_BLC_DIGITAL_OFFSET,
866		.type		= V4L2_CTRL_TYPE_INTEGER,
867		.name		= "BLC Digital Offset",
868		.min		= -2048,
869		.max		= 2047,
870		.step		= 1,
871		.def		= 40,
872		.flags		= 0,
873	}
874};
875
876/* -----------------------------------------------------------------------------
877 * V4L2 subdev core operations
878 */
879
880static int mt9p031_set_power(struct v4l2_subdev *subdev, int on)
881{
882	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
883	int ret = 0;
884
885	mutex_lock(&mt9p031->power_lock);
886
887	/* If the power count is modified from 0 to != 0 or from != 0 to 0,
888	 * update the power state.
889	 */
890	if (mt9p031->power_count == !on) {
891		ret = __mt9p031_set_power(mt9p031, !!on);
892		if (ret < 0)
893			goto out;
894	}
895
896	/* Update the power count. */
897	mt9p031->power_count += on ? 1 : -1;
898	WARN_ON(mt9p031->power_count < 0);
899
900out:
901	mutex_unlock(&mt9p031->power_lock);
902	return ret;
903}
904
905/* -----------------------------------------------------------------------------
906 * V4L2 subdev internal operations
907 */
908
909static int mt9p031_registered(struct v4l2_subdev *subdev)
910{
911	struct i2c_client *client = v4l2_get_subdevdata(subdev);
912	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
913	s32 data;
914	int ret;
915
916	ret = mt9p031_power_on(mt9p031);
917	if (ret < 0) {
918		dev_err(&client->dev, "MT9P031 power up failed\n");
919		return ret;
920	}
921
922	/* Read out the chip version register */
923	data = mt9p031_read(client, MT9P031_CHIP_VERSION);
924	mt9p031_power_off(mt9p031);
925
926	if (data != MT9P031_CHIP_VERSION_VALUE) {
927		dev_err(&client->dev, "MT9P031 not detected, wrong version "
928			"0x%04x\n", data);
929		return -ENODEV;
930	}
931
932	dev_info(&client->dev, "MT9P031 detected at address 0x%02x\n",
933		 client->addr);
934
935	return 0;
936}
937
938static int mt9p031_open(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
939{
940	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
941	struct v4l2_mbus_framefmt *format;
942	struct v4l2_rect *crop;
943
944	crop = v4l2_subdev_get_try_crop(fh, 0);
945	crop->left = MT9P031_COLUMN_START_DEF;
946	crop->top = MT9P031_ROW_START_DEF;
947	crop->width = MT9P031_WINDOW_WIDTH_DEF;
948	crop->height = MT9P031_WINDOW_HEIGHT_DEF;
949
950	format = v4l2_subdev_get_try_format(fh, 0);
951
952	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
953		format->code = V4L2_MBUS_FMT_Y12_1X12;
954	else
955		format->code = V4L2_MBUS_FMT_SGRBG12_1X12;
956
957	format->width = MT9P031_WINDOW_WIDTH_DEF;
958	format->height = MT9P031_WINDOW_HEIGHT_DEF;
959	format->field = V4L2_FIELD_NONE;
960	format->colorspace = V4L2_COLORSPACE_SRGB;
961
962	return mt9p031_set_power(subdev, 1);
963}
964
965static int mt9p031_close(struct v4l2_subdev *subdev, struct v4l2_subdev_fh *fh)
966{
967	return mt9p031_set_power(subdev, 0);
968}
969
970static struct v4l2_subdev_core_ops mt9p031_subdev_core_ops = {
971	.s_power        = mt9p031_set_power,
972};
973
974static struct v4l2_subdev_video_ops mt9p031_subdev_video_ops = {
975	.s_stream       = mt9p031_s_stream,
976};
977
978static struct v4l2_subdev_pad_ops mt9p031_subdev_pad_ops = {
979	.enum_mbus_code = mt9p031_enum_mbus_code,
980	.enum_frame_size = mt9p031_enum_frame_size,
981	.get_fmt = mt9p031_get_format,
982	.set_fmt = mt9p031_set_format,
983	.get_crop = mt9p031_get_crop,
984	.set_crop = mt9p031_set_crop,
985};
986
987static struct v4l2_subdev_ops mt9p031_subdev_ops = {
988	.core   = &mt9p031_subdev_core_ops,
989	.video  = &mt9p031_subdev_video_ops,
990	.pad    = &mt9p031_subdev_pad_ops,
991};
992
993static const struct v4l2_subdev_internal_ops mt9p031_subdev_internal_ops = {
994	.registered = mt9p031_registered,
995	.open = mt9p031_open,
996	.close = mt9p031_close,
997};
998
999/* -----------------------------------------------------------------------------
1000 * Driver initialization and probing
1001 */
1002
1003static struct mt9p031_platform_data *
1004mt9p031_get_pdata(struct i2c_client *client)
1005{
1006	struct mt9p031_platform_data *pdata;
1007	struct device_node *np;
1008
1009	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
1010		return client->dev.platform_data;
1011
1012	np = of_graph_get_next_endpoint(client->dev.of_node, NULL);
1013	if (!np)
1014		return NULL;
1015
1016	pdata = devm_kzalloc(&client->dev, sizeof(*pdata), GFP_KERNEL);
1017	if (!pdata)
1018		goto done;
1019
1020	pdata->reset = of_get_named_gpio(client->dev.of_node, "reset-gpios", 0);
1021	of_property_read_u32(np, "input-clock-frequency", &pdata->ext_freq);
1022	of_property_read_u32(np, "pixel-clock-frequency", &pdata->target_freq);
1023
1024done:
1025	of_node_put(np);
1026	return pdata;
1027}
1028
1029static int mt9p031_probe(struct i2c_client *client,
1030			 const struct i2c_device_id *did)
1031{
1032	struct mt9p031_platform_data *pdata = mt9p031_get_pdata(client);
1033	struct i2c_adapter *adapter = to_i2c_adapter(client->dev.parent);
1034	struct mt9p031 *mt9p031;
1035	unsigned int i;
1036	int ret;
1037
1038	if (pdata == NULL) {
1039		dev_err(&client->dev, "No platform data\n");
1040		return -EINVAL;
1041	}
1042
1043	if (!i2c_check_functionality(adapter, I2C_FUNC_SMBUS_WORD_DATA)) {
1044		dev_warn(&client->dev,
1045			"I2C-Adapter doesn't support I2C_FUNC_SMBUS_WORD\n");
1046		return -EIO;
1047	}
1048
1049	mt9p031 = devm_kzalloc(&client->dev, sizeof(*mt9p031), GFP_KERNEL);
1050	if (mt9p031 == NULL)
1051		return -ENOMEM;
1052
1053	mt9p031->pdata = pdata;
1054	mt9p031->output_control	= MT9P031_OUTPUT_CONTROL_DEF;
1055	mt9p031->mode2 = MT9P031_READ_MODE_2_ROW_BLC;
1056	mt9p031->model = did->driver_data;
1057	mt9p031->reset = -1;
1058
1059	mt9p031->regulators[0].supply = "vdd";
1060	mt9p031->regulators[1].supply = "vdd_io";
1061	mt9p031->regulators[2].supply = "vaa";
1062
1063	ret = devm_regulator_bulk_get(&client->dev, 3, mt9p031->regulators);
1064	if (ret < 0) {
1065		dev_err(&client->dev, "Unable to get regulators\n");
1066		return ret;
1067	}
1068
1069	v4l2_ctrl_handler_init(&mt9p031->ctrls, ARRAY_SIZE(mt9p031_ctrls) + 6);
1070
1071	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1072			  V4L2_CID_EXPOSURE, MT9P031_SHUTTER_WIDTH_MIN,
1073			  MT9P031_SHUTTER_WIDTH_MAX, 1,
1074			  MT9P031_SHUTTER_WIDTH_DEF);
1075	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1076			  V4L2_CID_GAIN, MT9P031_GLOBAL_GAIN_MIN,
1077			  MT9P031_GLOBAL_GAIN_MAX, 1, MT9P031_GLOBAL_GAIN_DEF);
1078	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1079			  V4L2_CID_HFLIP, 0, 1, 1, 0);
1080	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1081			  V4L2_CID_VFLIP, 0, 1, 1, 0);
1082	v4l2_ctrl_new_std(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1083			  V4L2_CID_PIXEL_RATE, pdata->target_freq,
1084			  pdata->target_freq, 1, pdata->target_freq);
1085	v4l2_ctrl_new_std_menu_items(&mt9p031->ctrls, &mt9p031_ctrl_ops,
1086			  V4L2_CID_TEST_PATTERN,
1087			  ARRAY_SIZE(mt9p031_test_pattern_menu) - 1, 0,
1088			  0, mt9p031_test_pattern_menu);
1089
1090	for (i = 0; i < ARRAY_SIZE(mt9p031_ctrls); ++i)
1091		v4l2_ctrl_new_custom(&mt9p031->ctrls, &mt9p031_ctrls[i], NULL);
1092
1093	mt9p031->subdev.ctrl_handler = &mt9p031->ctrls;
1094
1095	if (mt9p031->ctrls.error) {
1096		printk(KERN_INFO "%s: control initialization error %d\n",
1097		       __func__, mt9p031->ctrls.error);
1098		ret = mt9p031->ctrls.error;
1099		goto done;
1100	}
1101
1102	mt9p031->blc_auto = v4l2_ctrl_find(&mt9p031->ctrls, V4L2_CID_BLC_AUTO);
1103	mt9p031->blc_offset = v4l2_ctrl_find(&mt9p031->ctrls,
1104					     V4L2_CID_BLC_DIGITAL_OFFSET);
1105
1106	mutex_init(&mt9p031->power_lock);
1107	v4l2_i2c_subdev_init(&mt9p031->subdev, client, &mt9p031_subdev_ops);
1108	mt9p031->subdev.internal_ops = &mt9p031_subdev_internal_ops;
1109
1110	mt9p031->pad.flags = MEDIA_PAD_FL_SOURCE;
1111	ret = media_entity_init(&mt9p031->subdev.entity, 1, &mt9p031->pad, 0);
1112	if (ret < 0)
1113		goto done;
1114
1115	mt9p031->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1116
1117	mt9p031->crop.width = MT9P031_WINDOW_WIDTH_DEF;
1118	mt9p031->crop.height = MT9P031_WINDOW_HEIGHT_DEF;
1119	mt9p031->crop.left = MT9P031_COLUMN_START_DEF;
1120	mt9p031->crop.top = MT9P031_ROW_START_DEF;
1121
1122	if (mt9p031->model == MT9P031_MODEL_MONOCHROME)
1123		mt9p031->format.code = V4L2_MBUS_FMT_Y12_1X12;
1124	else
1125		mt9p031->format.code = V4L2_MBUS_FMT_SGRBG12_1X12;
1126
1127	mt9p031->format.width = MT9P031_WINDOW_WIDTH_DEF;
1128	mt9p031->format.height = MT9P031_WINDOW_HEIGHT_DEF;
1129	mt9p031->format.field = V4L2_FIELD_NONE;
1130	mt9p031->format.colorspace = V4L2_COLORSPACE_SRGB;
1131
1132	if (gpio_is_valid(pdata->reset)) {
1133		ret = devm_gpio_request_one(&client->dev, pdata->reset,
1134					    GPIOF_OUT_INIT_LOW, "mt9p031_rst");
1135		if (ret < 0)
1136			goto done;
1137
1138		mt9p031->reset = pdata->reset;
1139	}
1140
1141	ret = mt9p031_clk_setup(mt9p031);
1142
1143done:
1144	if (ret < 0) {
1145		v4l2_ctrl_handler_free(&mt9p031->ctrls);
1146		media_entity_cleanup(&mt9p031->subdev.entity);
1147	}
1148
1149	return ret;
1150}
1151
1152static int mt9p031_remove(struct i2c_client *client)
1153{
1154	struct v4l2_subdev *subdev = i2c_get_clientdata(client);
1155	struct mt9p031 *mt9p031 = to_mt9p031(subdev);
1156
1157	v4l2_ctrl_handler_free(&mt9p031->ctrls);
1158	v4l2_device_unregister_subdev(subdev);
1159	media_entity_cleanup(&subdev->entity);
1160
1161	return 0;
1162}
1163
1164static const struct i2c_device_id mt9p031_id[] = {
1165	{ "mt9p031", MT9P031_MODEL_COLOR },
1166	{ "mt9p031m", MT9P031_MODEL_MONOCHROME },
1167	{ }
1168};
1169MODULE_DEVICE_TABLE(i2c, mt9p031_id);
1170
1171#if IS_ENABLED(CONFIG_OF)
1172static const struct of_device_id mt9p031_of_match[] = {
1173	{ .compatible = "aptina,mt9p031", },
1174	{ .compatible = "aptina,mt9p031m", },
1175	{ /* sentinel */ },
1176};
1177MODULE_DEVICE_TABLE(of, mt9p031_of_match);
1178#endif
1179
1180static struct i2c_driver mt9p031_i2c_driver = {
1181	.driver = {
1182		.of_match_table = of_match_ptr(mt9p031_of_match),
1183		.name = "mt9p031",
1184	},
1185	.probe          = mt9p031_probe,
1186	.remove         = mt9p031_remove,
1187	.id_table       = mt9p031_id,
1188};
1189
1190module_i2c_driver(mt9p031_i2c_driver);
1191
1192MODULE_DESCRIPTION("Aptina MT9P031 Camera driver");
1193MODULE_AUTHOR("Bastian Hecht <hechtb@gmail.com>");
1194MODULE_LICENSE("GPL v2");
1195