1// SPDX-License-Identifier: GPL-2.0
2//
3// mt9v011 -Micron 1/4-Inch VGA Digital Image Sensor
4//
5// Copyright (c) 2009 Mauro Carvalho Chehab <mchehab@kernel.org>
6
7#include <linux/i2c.h>
8#include <linux/slab.h>
9#include <linux/videodev2.h>
10#include <linux/delay.h>
11#include <linux/module.h>
12#include <asm/div64.h>
13#include <media/v4l2-device.h>
14#include <media/v4l2-ctrls.h>
15#include <media/i2c/mt9v011.h>
16
17MODULE_DESCRIPTION("Micron mt9v011 sensor driver");
18MODULE_AUTHOR("Mauro Carvalho Chehab");
19MODULE_LICENSE("GPL v2");
20
21static int debug;
22module_param(debug, int, 0);
23MODULE_PARM_DESC(debug, "Debug level (0-2)");
24
25#define R00_MT9V011_CHIP_VERSION	0x00
26#define R01_MT9V011_ROWSTART		0x01
27#define R02_MT9V011_COLSTART		0x02
28#define R03_MT9V011_HEIGHT		0x03
29#define R04_MT9V011_WIDTH		0x04
30#define R05_MT9V011_HBLANK		0x05
31#define R06_MT9V011_VBLANK		0x06
32#define R07_MT9V011_OUT_CTRL		0x07
33#define R09_MT9V011_SHUTTER_WIDTH	0x09
34#define R0A_MT9V011_CLK_SPEED		0x0a
35#define R0B_MT9V011_RESTART		0x0b
36#define R0C_MT9V011_SHUTTER_DELAY	0x0c
37#define R0D_MT9V011_RESET		0x0d
38#define R1E_MT9V011_DIGITAL_ZOOM	0x1e
39#define R20_MT9V011_READ_MODE		0x20
40#define R2B_MT9V011_GREEN_1_GAIN	0x2b
41#define R2C_MT9V011_BLUE_GAIN		0x2c
42#define R2D_MT9V011_RED_GAIN		0x2d
43#define R2E_MT9V011_GREEN_2_GAIN	0x2e
44#define R35_MT9V011_GLOBAL_GAIN		0x35
45#define RF1_MT9V011_CHIP_ENABLE		0xf1
46
47#define MT9V011_VERSION			0x8232
48#define MT9V011_REV_B_VERSION		0x8243
49
50struct mt9v011 {
51	struct v4l2_subdev sd;
52	struct media_pad pad;
53	struct v4l2_ctrl_handler ctrls;
54	unsigned width, height;
55	unsigned xtal;
56	unsigned hflip:1;
57	unsigned vflip:1;
58
59	u16 global_gain, exposure;
60	s16 red_bal, blue_bal;
61};
62
63static inline struct mt9v011 *to_mt9v011(struct v4l2_subdev *sd)
64{
65	return container_of(sd, struct mt9v011, sd);
66}
67
68static int mt9v011_read(struct v4l2_subdev *sd, unsigned char addr)
69{
70	struct i2c_client *c = v4l2_get_subdevdata(sd);
71	__be16 buffer;
72	int rc, val;
73
74	rc = i2c_master_send(c, &addr, 1);
75	if (rc != 1)
76		v4l2_dbg(0, debug, sd,
77			 "i2c i/o error: rc == %d (should be 1)\n", rc);
78
79	msleep(10);
80
81	rc = i2c_master_recv(c, (char *)&buffer, 2);
82	if (rc != 2)
83		v4l2_dbg(0, debug, sd,
84			 "i2c i/o error: rc == %d (should be 2)\n", rc);
85
86	val = be16_to_cpu(buffer);
87
88	v4l2_dbg(2, debug, sd, "mt9v011: read 0x%02x = 0x%04x\n", addr, val);
89
90	return val;
91}
92
93static void mt9v011_write(struct v4l2_subdev *sd, unsigned char addr,
94				 u16 value)
95{
96	struct i2c_client *c = v4l2_get_subdevdata(sd);
97	unsigned char buffer[3];
98	int rc;
99
100	buffer[0] = addr;
101	buffer[1] = value >> 8;
102	buffer[2] = value & 0xff;
103
104	v4l2_dbg(2, debug, sd,
105		 "mt9v011: writing 0x%02x 0x%04x\n", buffer[0], value);
106	rc = i2c_master_send(c, buffer, 3);
107	if (rc != 3)
108		v4l2_dbg(0, debug, sd,
109			 "i2c i/o error: rc == %d (should be 3)\n", rc);
110}
111
112
113struct i2c_reg_value {
114	unsigned char reg;
115	u16           value;
116};
117
118/*
119 * Values used at the original driver
120 * Some values are marked as Reserved at the datasheet
121 */
122static const struct i2c_reg_value mt9v011_init_default[] = {
123		{ R0D_MT9V011_RESET, 0x0001 },
124		{ R0D_MT9V011_RESET, 0x0000 },
125
126		{ R0C_MT9V011_SHUTTER_DELAY, 0x0000 },
127		{ R09_MT9V011_SHUTTER_WIDTH, 0x1fc },
128
129		{ R0A_MT9V011_CLK_SPEED, 0x0000 },
130		{ R1E_MT9V011_DIGITAL_ZOOM,  0x0000 },
131
132		{ R07_MT9V011_OUT_CTRL, 0x0002 },	/* chip enable */
133};
134
135
136static u16 calc_mt9v011_gain(s16 lineargain)
137{
138
139	u16 digitalgain = 0;
140	u16 analogmult = 0;
141	u16 analoginit = 0;
142
143	if (lineargain < 0)
144		lineargain = 0;
145
146	/* recommended minimum */
147	lineargain += 0x0020;
148
149	if (lineargain > 2047)
150		lineargain = 2047;
151
152	if (lineargain > 1023) {
153		digitalgain = 3;
154		analogmult = 3;
155		analoginit = lineargain / 16;
156	} else if (lineargain > 511) {
157		digitalgain = 1;
158		analogmult = 3;
159		analoginit = lineargain / 8;
160	} else if (lineargain > 255) {
161		analogmult = 3;
162		analoginit = lineargain / 4;
163	} else if (lineargain > 127) {
164		analogmult = 1;
165		analoginit = lineargain / 2;
166	} else
167		analoginit = lineargain;
168
169	return analoginit + (analogmult << 7) + (digitalgain << 9);
170
171}
172
173static void set_balance(struct v4l2_subdev *sd)
174{
175	struct mt9v011 *core = to_mt9v011(sd);
176	u16 green_gain, blue_gain, red_gain;
177	u16 exposure;
178	s16 bal;
179
180	exposure = core->exposure;
181
182	green_gain = calc_mt9v011_gain(core->global_gain);
183
184	bal = core->global_gain;
185	bal += (core->blue_bal * core->global_gain / (1 << 7));
186	blue_gain = calc_mt9v011_gain(bal);
187
188	bal = core->global_gain;
189	bal += (core->red_bal * core->global_gain / (1 << 7));
190	red_gain = calc_mt9v011_gain(bal);
191
192	mt9v011_write(sd, R2B_MT9V011_GREEN_1_GAIN, green_gain);
193	mt9v011_write(sd, R2E_MT9V011_GREEN_2_GAIN, green_gain);
194	mt9v011_write(sd, R2C_MT9V011_BLUE_GAIN, blue_gain);
195	mt9v011_write(sd, R2D_MT9V011_RED_GAIN, red_gain);
196	mt9v011_write(sd, R09_MT9V011_SHUTTER_WIDTH, exposure);
197}
198
199static void calc_fps(struct v4l2_subdev *sd, u32 *numerator, u32 *denominator)
200{
201	struct mt9v011 *core = to_mt9v011(sd);
202	unsigned height, width, hblank, vblank, speed;
203	unsigned row_time, t_time;
204	u64 frames_per_ms;
205	unsigned tmp;
206
207	height = mt9v011_read(sd, R03_MT9V011_HEIGHT);
208	width = mt9v011_read(sd, R04_MT9V011_WIDTH);
209	hblank = mt9v011_read(sd, R05_MT9V011_HBLANK);
210	vblank = mt9v011_read(sd, R06_MT9V011_VBLANK);
211	speed = mt9v011_read(sd, R0A_MT9V011_CLK_SPEED);
212
213	row_time = (width + 113 + hblank) * (speed + 2);
214	t_time = row_time * (height + vblank + 1);
215
216	frames_per_ms = core->xtal * 1000l;
217	do_div(frames_per_ms, t_time);
218	tmp = frames_per_ms;
219
220	v4l2_dbg(1, debug, sd, "Programmed to %u.%03u fps (%d pixel clcks)\n",
221		tmp / 1000, tmp % 1000, t_time);
222
223	if (numerator && denominator) {
224		*numerator = 1000;
225		*denominator = (u32)frames_per_ms;
226	}
227}
228
229static u16 calc_speed(struct v4l2_subdev *sd, u32 numerator, u32 denominator)
230{
231	struct mt9v011 *core = to_mt9v011(sd);
232	unsigned height, width, hblank, vblank;
233	unsigned row_time, line_time;
234	u64 t_time, speed;
235
236	/* Avoid bogus calculus */
237	if (!numerator || !denominator)
238		return 0;
239
240	height = mt9v011_read(sd, R03_MT9V011_HEIGHT);
241	width = mt9v011_read(sd, R04_MT9V011_WIDTH);
242	hblank = mt9v011_read(sd, R05_MT9V011_HBLANK);
243	vblank = mt9v011_read(sd, R06_MT9V011_VBLANK);
244
245	row_time = width + 113 + hblank;
246	line_time = height + vblank + 1;
247
248	t_time = core->xtal * ((u64)numerator);
249	/* round to the closest value */
250	t_time += denominator / 2;
251	do_div(t_time, denominator);
252
253	speed = t_time;
254	do_div(speed, row_time * line_time);
255
256	/* Avoid having a negative value for speed */
257	if (speed < 2)
258		speed = 0;
259	else
260		speed -= 2;
261
262	/* Avoid speed overflow */
263	if (speed > 15)
264		return 15;
265
266	return (u16)speed;
267}
268
269static void set_res(struct v4l2_subdev *sd)
270{
271	struct mt9v011 *core = to_mt9v011(sd);
272	unsigned vstart, hstart;
273
274	/*
275	 * The mt9v011 doesn't have scaling. So, in order to select the desired
276	 * resolution, we're cropping at the middle of the sensor.
277	 * hblank and vblank should be adjusted, in order to warrant that
278	 * we'll preserve the line timings for 30 fps, no matter what resolution
279	 * is selected.
280	 * NOTE: datasheet says that width (and height) should be filled with
281	 * width-1. However, this doesn't work, since one pixel per line will
282	 * be missing.
283	 */
284
285	hstart = 20 + (640 - core->width) / 2;
286	mt9v011_write(sd, R02_MT9V011_COLSTART, hstart);
287	mt9v011_write(sd, R04_MT9V011_WIDTH, core->width);
288	mt9v011_write(sd, R05_MT9V011_HBLANK, 771 - core->width);
289
290	vstart = 8 + (480 - core->height) / 2;
291	mt9v011_write(sd, R01_MT9V011_ROWSTART, vstart);
292	mt9v011_write(sd, R03_MT9V011_HEIGHT, core->height);
293	mt9v011_write(sd, R06_MT9V011_VBLANK, 508 - core->height);
294
295	calc_fps(sd, NULL, NULL);
296};
297
298static void set_read_mode(struct v4l2_subdev *sd)
299{
300	struct mt9v011 *core = to_mt9v011(sd);
301	unsigned mode = 0x1000;
302
303	if (core->hflip)
304		mode |= 0x4000;
305
306	if (core->vflip)
307		mode |= 0x8000;
308
309	mt9v011_write(sd, R20_MT9V011_READ_MODE, mode);
310}
311
312static int mt9v011_reset(struct v4l2_subdev *sd, u32 val)
313{
314	int i;
315
316	for (i = 0; i < ARRAY_SIZE(mt9v011_init_default); i++)
317		mt9v011_write(sd, mt9v011_init_default[i].reg,
318			       mt9v011_init_default[i].value);
319
320	set_balance(sd);
321	set_res(sd);
322	set_read_mode(sd);
323
324	return 0;
325}
326
327static int mt9v011_enum_mbus_code(struct v4l2_subdev *sd,
328		struct v4l2_subdev_state *sd_state,
329		struct v4l2_subdev_mbus_code_enum *code)
330{
331	if (code->pad || code->index > 0)
332		return -EINVAL;
333
334	code->code = MEDIA_BUS_FMT_SGRBG8_1X8;
335	return 0;
336}
337
338static int mt9v011_set_fmt(struct v4l2_subdev *sd,
339		struct v4l2_subdev_state *sd_state,
340		struct v4l2_subdev_format *format)
341{
342	struct v4l2_mbus_framefmt *fmt = &format->format;
343	struct mt9v011 *core = to_mt9v011(sd);
344
345	if (format->pad || fmt->code != MEDIA_BUS_FMT_SGRBG8_1X8)
346		return -EINVAL;
347
348	v4l_bound_align_image(&fmt->width, 48, 639, 1,
349			      &fmt->height, 32, 480, 1, 0);
350	fmt->field = V4L2_FIELD_NONE;
351	fmt->colorspace = V4L2_COLORSPACE_SRGB;
352
353	if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
354		core->width = fmt->width;
355		core->height = fmt->height;
356
357		set_res(sd);
358	} else {
359		*v4l2_subdev_state_get_format(sd_state, 0) = *fmt;
360	}
361
362	return 0;
363}
364
365static int mt9v011_get_frame_interval(struct v4l2_subdev *sd,
366				      struct v4l2_subdev_state *sd_state,
367				      struct v4l2_subdev_frame_interval *ival)
368{
369	/*
370	 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
371	 * subdev active state API.
372	 */
373	if (ival->which != V4L2_SUBDEV_FORMAT_ACTIVE)
374		return -EINVAL;
375
376	calc_fps(sd,
377		 &ival->interval.numerator,
378		 &ival->interval.denominator);
379
380	return 0;
381}
382
383static int mt9v011_set_frame_interval(struct v4l2_subdev *sd,
384				      struct v4l2_subdev_state *sd_state,
385				      struct v4l2_subdev_frame_interval *ival)
386{
387	struct v4l2_fract *tpf = &ival->interval;
388	u16 speed;
389
390	/*
391	 * FIXME: Implement support for V4L2_SUBDEV_FORMAT_TRY, using the V4L2
392	 * subdev active state API.
393	 */
394	if (ival->which != V4L2_SUBDEV_FORMAT_ACTIVE)
395		return -EINVAL;
396
397	speed = calc_speed(sd, tpf->numerator, tpf->denominator);
398
399	mt9v011_write(sd, R0A_MT9V011_CLK_SPEED, speed);
400	v4l2_dbg(1, debug, sd, "Setting speed to %d\n", speed);
401
402	/* Recalculate and update fps info */
403	calc_fps(sd, &tpf->numerator, &tpf->denominator);
404
405	return 0;
406}
407
408#ifdef CONFIG_VIDEO_ADV_DEBUG
409static int mt9v011_g_register(struct v4l2_subdev *sd,
410			      struct v4l2_dbg_register *reg)
411{
412	reg->val = mt9v011_read(sd, reg->reg & 0xff);
413	reg->size = 2;
414
415	return 0;
416}
417
418static int mt9v011_s_register(struct v4l2_subdev *sd,
419			      const struct v4l2_dbg_register *reg)
420{
421	mt9v011_write(sd, reg->reg & 0xff, reg->val & 0xffff);
422
423	return 0;
424}
425#endif
426
427static int mt9v011_s_ctrl(struct v4l2_ctrl *ctrl)
428{
429	struct mt9v011 *core =
430		container_of(ctrl->handler, struct mt9v011, ctrls);
431	struct v4l2_subdev *sd = &core->sd;
432
433	switch (ctrl->id) {
434	case V4L2_CID_GAIN:
435		core->global_gain = ctrl->val;
436		break;
437	case V4L2_CID_EXPOSURE:
438		core->exposure = ctrl->val;
439		break;
440	case V4L2_CID_RED_BALANCE:
441		core->red_bal = ctrl->val;
442		break;
443	case V4L2_CID_BLUE_BALANCE:
444		core->blue_bal = ctrl->val;
445		break;
446	case V4L2_CID_HFLIP:
447		core->hflip = ctrl->val;
448		set_read_mode(sd);
449		return 0;
450	case V4L2_CID_VFLIP:
451		core->vflip = ctrl->val;
452		set_read_mode(sd);
453		return 0;
454	default:
455		return -EINVAL;
456	}
457
458	set_balance(sd);
459	return 0;
460}
461
462static const struct v4l2_ctrl_ops mt9v011_ctrl_ops = {
463	.s_ctrl = mt9v011_s_ctrl,
464};
465
466static const struct v4l2_subdev_core_ops mt9v011_core_ops = {
467	.reset = mt9v011_reset,
468#ifdef CONFIG_VIDEO_ADV_DEBUG
469	.g_register = mt9v011_g_register,
470	.s_register = mt9v011_s_register,
471#endif
472};
473
474static const struct v4l2_subdev_pad_ops mt9v011_pad_ops = {
475	.enum_mbus_code = mt9v011_enum_mbus_code,
476	.set_fmt = mt9v011_set_fmt,
477	.get_frame_interval = mt9v011_get_frame_interval,
478	.set_frame_interval = mt9v011_set_frame_interval,
479};
480
481static const struct v4l2_subdev_ops mt9v011_ops = {
482	.core  = &mt9v011_core_ops,
483	.pad   = &mt9v011_pad_ops,
484};
485
486
487/****************************************************************************
488			I2C Client & Driver
489 ****************************************************************************/
490
491static int mt9v011_probe(struct i2c_client *c)
492{
493	u16 version;
494	struct mt9v011 *core;
495	struct v4l2_subdev *sd;
496	int ret;
497
498	/* Check if the adapter supports the needed features */
499	if (!i2c_check_functionality(c->adapter,
500	     I2C_FUNC_SMBUS_READ_BYTE | I2C_FUNC_SMBUS_WRITE_BYTE_DATA))
501		return -EIO;
502
503	core = devm_kzalloc(&c->dev, sizeof(struct mt9v011), GFP_KERNEL);
504	if (!core)
505		return -ENOMEM;
506
507	sd = &core->sd;
508	v4l2_i2c_subdev_init(sd, c, &mt9v011_ops);
509
510	core->pad.flags = MEDIA_PAD_FL_SOURCE;
511	sd->entity.function = MEDIA_ENT_F_CAM_SENSOR;
512
513	ret = media_entity_pads_init(&sd->entity, 1, &core->pad);
514	if (ret < 0)
515		return ret;
516
517	/* Check if the sensor is really a MT9V011 */
518	version = mt9v011_read(sd, R00_MT9V011_CHIP_VERSION);
519	if ((version != MT9V011_VERSION) &&
520	    (version != MT9V011_REV_B_VERSION)) {
521		v4l2_info(sd, "*** unknown micron chip detected (0x%04x).\n",
522			  version);
523		return -EINVAL;
524	}
525
526	v4l2_ctrl_handler_init(&core->ctrls, 5);
527	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
528			  V4L2_CID_GAIN, 0, (1 << 12) - 1 - 0x20, 1, 0x20);
529	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
530			  V4L2_CID_EXPOSURE, 0, 2047, 1, 0x01fc);
531	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
532			  V4L2_CID_RED_BALANCE, -(1 << 9), (1 << 9) - 1, 1, 0);
533	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
534			  V4L2_CID_BLUE_BALANCE, -(1 << 9), (1 << 9) - 1, 1, 0);
535	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
536			  V4L2_CID_HFLIP, 0, 1, 1, 0);
537	v4l2_ctrl_new_std(&core->ctrls, &mt9v011_ctrl_ops,
538			  V4L2_CID_VFLIP, 0, 1, 1, 0);
539
540	if (core->ctrls.error) {
541		int ret = core->ctrls.error;
542
543		v4l2_err(sd, "control initialization error %d\n", ret);
544		v4l2_ctrl_handler_free(&core->ctrls);
545		return ret;
546	}
547	core->sd.ctrl_handler = &core->ctrls;
548
549	core->global_gain = 0x0024;
550	core->exposure = 0x01fc;
551	core->width  = 640;
552	core->height = 480;
553	core->xtal = 27000000;	/* Hz */
554
555	if (c->dev.platform_data) {
556		struct mt9v011_platform_data *pdata = c->dev.platform_data;
557
558		core->xtal = pdata->xtal;
559		v4l2_dbg(1, debug, sd, "xtal set to %d.%03d MHz\n",
560			core->xtal / 1000000, (core->xtal / 1000) % 1000);
561	}
562
563	v4l_info(c, "chip found @ 0x%02x (%s - chip version 0x%04x)\n",
564		 c->addr << 1, c->adapter->name, version);
565
566	return 0;
567}
568
569static void mt9v011_remove(struct i2c_client *c)
570{
571	struct v4l2_subdev *sd = i2c_get_clientdata(c);
572	struct mt9v011 *core = to_mt9v011(sd);
573
574	v4l2_dbg(1, debug, sd,
575		"mt9v011.c: removing mt9v011 adapter on address 0x%x\n",
576		c->addr << 1);
577
578	v4l2_device_unregister_subdev(sd);
579	v4l2_ctrl_handler_free(&core->ctrls);
580}
581
582/* ----------------------------------------------------------------------- */
583
584static const struct i2c_device_id mt9v011_id[] = {
585	{ "mt9v011", 0 },
586	{ }
587};
588MODULE_DEVICE_TABLE(i2c, mt9v011_id);
589
590static struct i2c_driver mt9v011_driver = {
591	.driver = {
592		.name	= "mt9v011",
593	},
594	.probe		= mt9v011_probe,
595	.remove		= mt9v011_remove,
596	.id_table	= mt9v011_id,
597};
598
599module_i2c_driver(mt9v011_driver);
600