1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
20
21#include "m5602_po1030.h"
22
23static int po1030_s_ctrl(struct v4l2_ctrl *ctrl);
24static void po1030_dump_registers(struct sd *sd);
25
26static struct v4l2_pix_format po1030_modes[] = {
27 {
28 640,
29 480,
30 V4L2_PIX_FMT_SBGGR8,
31 V4L2_FIELD_NONE,
32 .sizeimage = 640 * 480,
33 .bytesperline = 640,
34 .colorspace = V4L2_COLORSPACE_SRGB,
35 .priv = 2
36 }
37};
38
39static const struct v4l2_ctrl_ops po1030_ctrl_ops = {
40 .s_ctrl = po1030_s_ctrl,
41};
42
43static const struct v4l2_ctrl_config po1030_greenbal_cfg = {
44 .ops = &po1030_ctrl_ops,
45 .id = M5602_V4L2_CID_GREEN_BALANCE,
46 .name = "Green Balance",
47 .type = V4L2_CTRL_TYPE_INTEGER,
48 .min = 0,
49 .max = 255,
50 .step = 1,
51 .def = PO1030_GREEN_GAIN_DEFAULT,
52 .flags = V4L2_CTRL_FLAG_SLIDER,
53};
54
55int po1030_probe(struct sd *sd)
56{
57 u8 dev_id_h = 0, i;
58 struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
59
60 if (force_sensor) {
61 if (force_sensor == PO1030_SENSOR) {
62 pr_info("Forcing a %s sensor\n", po1030.name);
63 goto sensor_found;
64 }
65
66
67 return -ENODEV;
68 }
69
70 PDEBUG(D_PROBE, "Probing for a po1030 sensor");
71
72
73 for (i = 0; i < ARRAY_SIZE(preinit_po1030); i++) {
74 u8 data = preinit_po1030[i][2];
75 if (preinit_po1030[i][0] == SENSOR)
76 m5602_write_sensor(sd,
77 preinit_po1030[i][1], &data, 1);
78 else
79 m5602_write_bridge(sd, preinit_po1030[i][1], data);
80 }
81
82 if (m5602_read_sensor(sd, PO1030_DEVID_H, &dev_id_h, 1))
83 return -ENODEV;
84
85 if (dev_id_h == 0x30) {
86 pr_info("Detected a po1030 sensor\n");
87 goto sensor_found;
88 }
89 return -ENODEV;
90
91sensor_found:
92 sd->gspca_dev.cam.cam_mode = po1030_modes;
93 sd->gspca_dev.cam.nmodes = ARRAY_SIZE(po1030_modes);
94
95 return 0;
96}
97
98int po1030_init(struct sd *sd)
99{
100 int i, err = 0;
101
102
103 for (i = 0; i < ARRAY_SIZE(init_po1030) && !err; i++) {
104 u8 data[2] = {0x00, 0x00};
105
106 switch (init_po1030[i][0]) {
107 case BRIDGE:
108 err = m5602_write_bridge(sd,
109 init_po1030[i][1],
110 init_po1030[i][2]);
111 break;
112
113 case SENSOR:
114 data[0] = init_po1030[i][2];
115 err = m5602_write_sensor(sd,
116 init_po1030[i][1], data, 1);
117 break;
118
119 default:
120 pr_info("Invalid stream command, exiting init\n");
121 return -EINVAL;
122 }
123 }
124 if (err < 0)
125 return err;
126
127 if (dump_sensor)
128 po1030_dump_registers(sd);
129
130 return 0;
131}
132
133int po1030_init_controls(struct sd *sd)
134{
135 struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
136
137 sd->gspca_dev.vdev.ctrl_handler = hdl;
138 v4l2_ctrl_handler_init(hdl, 9);
139
140 sd->auto_white_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
141 V4L2_CID_AUTO_WHITE_BALANCE,
142 0, 1, 1, 0);
143 sd->green_bal = v4l2_ctrl_new_custom(hdl, &po1030_greenbal_cfg, NULL);
144 sd->red_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
145 V4L2_CID_RED_BALANCE, 0, 255, 1,
146 PO1030_RED_GAIN_DEFAULT);
147 sd->blue_bal = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops,
148 V4L2_CID_BLUE_BALANCE, 0, 255, 1,
149 PO1030_BLUE_GAIN_DEFAULT);
150
151 sd->autoexpo = v4l2_ctrl_new_std_menu(hdl, &po1030_ctrl_ops,
152 V4L2_CID_EXPOSURE_AUTO, 1, 0, V4L2_EXPOSURE_MANUAL);
153 sd->expo = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_EXPOSURE,
154 0, 0x2ff, 1, PO1030_EXPOSURE_DEFAULT);
155
156 sd->gain = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_GAIN, 0,
157 0x4f, 1, PO1030_GLOBAL_GAIN_DEFAULT);
158
159 sd->hflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_HFLIP,
160 0, 1, 1, 0);
161 sd->vflip = v4l2_ctrl_new_std(hdl, &po1030_ctrl_ops, V4L2_CID_VFLIP,
162 0, 1, 1, 0);
163
164 if (hdl->error) {
165 pr_err("Could not initialize controls\n");
166 return hdl->error;
167 }
168
169 v4l2_ctrl_auto_cluster(4, &sd->auto_white_bal, 0, false);
170 v4l2_ctrl_auto_cluster(2, &sd->autoexpo, 0, false);
171 v4l2_ctrl_cluster(2, &sd->hflip);
172
173 return 0;
174}
175
176int po1030_start(struct sd *sd)
177{
178 struct cam *cam = &sd->gspca_dev.cam;
179 int i, err = 0;
180 int width = cam->cam_mode[sd->gspca_dev.curr_mode].width;
181 int height = cam->cam_mode[sd->gspca_dev.curr_mode].height;
182 int ver_offs = cam->cam_mode[sd->gspca_dev.curr_mode].priv;
183 u8 data;
184
185 switch (width) {
186 case 320:
187 data = PO1030_SUBSAMPLING;
188 err = m5602_write_sensor(sd, PO1030_CONTROL3, &data, 1);
189 if (err < 0)
190 return err;
191
192 data = ((width + 3) >> 8) & 0xff;
193 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_H, &data, 1);
194 if (err < 0)
195 return err;
196
197 data = (width + 3) & 0xff;
198 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_L, &data, 1);
199 if (err < 0)
200 return err;
201
202 data = ((height + 1) >> 8) & 0xff;
203 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_H, &data, 1);
204 if (err < 0)
205 return err;
206
207 data = (height + 1) & 0xff;
208 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_L, &data, 1);
209
210 height += 6;
211 width -= 1;
212 break;
213
214 case 640:
215 data = 0;
216 err = m5602_write_sensor(sd, PO1030_CONTROL3, &data, 1);
217 if (err < 0)
218 return err;
219
220 data = ((width + 7) >> 8) & 0xff;
221 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_H, &data, 1);
222 if (err < 0)
223 return err;
224
225 data = (width + 7) & 0xff;
226 err = m5602_write_sensor(sd, PO1030_WINDOWWIDTH_L, &data, 1);
227 if (err < 0)
228 return err;
229
230 data = ((height + 3) >> 8) & 0xff;
231 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_H, &data, 1);
232 if (err < 0)
233 return err;
234
235 data = (height + 3) & 0xff;
236 err = m5602_write_sensor(sd, PO1030_WINDOWHEIGHT_L, &data, 1);
237
238 height += 12;
239 width -= 2;
240 break;
241 }
242 err = m5602_write_bridge(sd, M5602_XB_SENSOR_TYPE, 0x0c);
243 if (err < 0)
244 return err;
245
246 err = m5602_write_bridge(sd, M5602_XB_LINE_OF_FRAME_H, 0x81);
247 if (err < 0)
248 return err;
249
250 err = m5602_write_bridge(sd, M5602_XB_PIX_OF_LINE_H, 0x82);
251 if (err < 0)
252 return err;
253
254 err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0x01);
255 if (err < 0)
256 return err;
257
258 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA,
259 ((ver_offs >> 8) & 0xff));
260 if (err < 0)
261 return err;
262
263 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (ver_offs & 0xff));
264 if (err < 0)
265 return err;
266
267 for (i = 0; i < 2 && !err; i++)
268 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, 0);
269 if (err < 0)
270 return err;
271
272 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (height >> 8) & 0xff);
273 if (err < 0)
274 return err;
275
276 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, (height & 0xff));
277 if (err < 0)
278 return err;
279
280 for (i = 0; i < 2 && !err; i++)
281 err = m5602_write_bridge(sd, M5602_XB_VSYNC_PARA, 0);
282
283 for (i = 0; i < 2 && !err; i++)
284 err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0);
285
286 for (i = 0; i < 2 && !err; i++)
287 err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, 0);
288 if (err < 0)
289 return err;
290
291 err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, (width >> 8) & 0xff);
292 if (err < 0)
293 return err;
294
295 err = m5602_write_bridge(sd, M5602_XB_HSYNC_PARA, (width & 0xff));
296 if (err < 0)
297 return err;
298
299 err = m5602_write_bridge(sd, M5602_XB_SIG_INI, 0);
300 return err;
301}
302
303static int po1030_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
304{
305 struct sd *sd = (struct sd *) gspca_dev;
306 u8 i2c_data;
307 int err;
308
309 PDEBUG(D_CONF, "Set exposure to %d", val & 0xffff);
310
311 i2c_data = ((val & 0xff00) >> 8);
312 PDEBUG(D_CONF, "Set exposure to high byte to 0x%x",
313 i2c_data);
314
315 err = m5602_write_sensor(sd, PO1030_INTEGLINES_H,
316 &i2c_data, 1);
317 if (err < 0)
318 return err;
319
320 i2c_data = (val & 0xff);
321 PDEBUG(D_CONF, "Set exposure to low byte to 0x%x",
322 i2c_data);
323 err = m5602_write_sensor(sd, PO1030_INTEGLINES_M,
324 &i2c_data, 1);
325
326 return err;
327}
328
329static int po1030_set_gain(struct gspca_dev *gspca_dev, __s32 val)
330{
331 struct sd *sd = (struct sd *) gspca_dev;
332 u8 i2c_data;
333 int err;
334
335 i2c_data = val & 0xff;
336 PDEBUG(D_CONF, "Set global gain to %d", i2c_data);
337 err = m5602_write_sensor(sd, PO1030_GLOBALGAIN,
338 &i2c_data, 1);
339 return err;
340}
341
342static int po1030_set_hvflip(struct gspca_dev *gspca_dev)
343{
344 struct sd *sd = (struct sd *) gspca_dev;
345 u8 i2c_data;
346 int err;
347
348 PDEBUG(D_CONF, "Set hvflip %d %d", sd->hflip->val, sd->vflip->val);
349 err = m5602_read_sensor(sd, PO1030_CONTROL2, &i2c_data, 1);
350 if (err < 0)
351 return err;
352
353 i2c_data = (0x3f & i2c_data) | (sd->hflip->val << 7) |
354 (sd->vflip->val << 6);
355
356 err = m5602_write_sensor(sd, PO1030_CONTROL2,
357 &i2c_data, 1);
358
359 return err;
360}
361
362static int po1030_set_red_balance(struct gspca_dev *gspca_dev, __s32 val)
363{
364 struct sd *sd = (struct sd *) gspca_dev;
365 u8 i2c_data;
366 int err;
367
368 i2c_data = val & 0xff;
369 PDEBUG(D_CONF, "Set red gain to %d", i2c_data);
370 err = m5602_write_sensor(sd, PO1030_RED_GAIN,
371 &i2c_data, 1);
372 return err;
373}
374
375static int po1030_set_blue_balance(struct gspca_dev *gspca_dev, __s32 val)
376{
377 struct sd *sd = (struct sd *) gspca_dev;
378 u8 i2c_data;
379 int err;
380
381 i2c_data = val & 0xff;
382 PDEBUG(D_CONF, "Set blue gain to %d", i2c_data);
383 err = m5602_write_sensor(sd, PO1030_BLUE_GAIN,
384 &i2c_data, 1);
385
386 return err;
387}
388
389static int po1030_set_green_balance(struct gspca_dev *gspca_dev, __s32 val)
390{
391 struct sd *sd = (struct sd *) gspca_dev;
392 u8 i2c_data;
393 int err;
394
395 i2c_data = val & 0xff;
396 PDEBUG(D_CONF, "Set green gain to %d", i2c_data);
397
398 err = m5602_write_sensor(sd, PO1030_GREEN_1_GAIN,
399 &i2c_data, 1);
400 if (err < 0)
401 return err;
402
403 return m5602_write_sensor(sd, PO1030_GREEN_2_GAIN,
404 &i2c_data, 1);
405}
406
407static int po1030_set_auto_white_balance(struct gspca_dev *gspca_dev,
408 __s32 val)
409{
410 struct sd *sd = (struct sd *) gspca_dev;
411 u8 i2c_data;
412 int err;
413
414 err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
415 if (err < 0)
416 return err;
417
418 PDEBUG(D_CONF, "Set auto white balance to %d", val);
419 i2c_data = (i2c_data & 0xfe) | (val & 0x01);
420 err = m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
421 return err;
422}
423
424static int po1030_set_auto_exposure(struct gspca_dev *gspca_dev,
425 __s32 val)
426{
427 struct sd *sd = (struct sd *) gspca_dev;
428 u8 i2c_data;
429 int err;
430
431 err = m5602_read_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
432 if (err < 0)
433 return err;
434
435 PDEBUG(D_CONF, "Set auto exposure to %d", val);
436 val = (val == V4L2_EXPOSURE_AUTO);
437 i2c_data = (i2c_data & 0xfd) | ((val & 0x01) << 1);
438 return m5602_write_sensor(sd, PO1030_AUTOCTRL1, &i2c_data, 1);
439}
440
441void po1030_disconnect(struct sd *sd)
442{
443 sd->sensor = NULL;
444}
445
446static int po1030_s_ctrl(struct v4l2_ctrl *ctrl)
447{
448 struct gspca_dev *gspca_dev =
449 container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
450 struct sd *sd = (struct sd *) gspca_dev;
451 int err;
452
453 if (!gspca_dev->streaming)
454 return 0;
455
456 switch (ctrl->id) {
457 case V4L2_CID_AUTO_WHITE_BALANCE:
458 err = po1030_set_auto_white_balance(gspca_dev, ctrl->val);
459 if (err || ctrl->val)
460 return err;
461 err = po1030_set_green_balance(gspca_dev, sd->green_bal->val);
462 if (err)
463 return err;
464 err = po1030_set_red_balance(gspca_dev, sd->red_bal->val);
465 if (err)
466 return err;
467 err = po1030_set_blue_balance(gspca_dev, sd->blue_bal->val);
468 break;
469 case V4L2_CID_EXPOSURE_AUTO:
470 err = po1030_set_auto_exposure(gspca_dev, ctrl->val);
471 if (err || ctrl->val == V4L2_EXPOSURE_AUTO)
472 return err;
473 err = po1030_set_exposure(gspca_dev, sd->expo->val);
474 break;
475 case V4L2_CID_GAIN:
476 err = po1030_set_gain(gspca_dev, ctrl->val);
477 break;
478 case V4L2_CID_HFLIP:
479 err = po1030_set_hvflip(gspca_dev);
480 break;
481 default:
482 return -EINVAL;
483 }
484
485 return err;
486}
487
488static void po1030_dump_registers(struct sd *sd)
489{
490 int address;
491 u8 value = 0;
492
493 pr_info("Dumping the po1030 sensor core registers\n");
494 for (address = 0; address < 0x7f; address++) {
495 m5602_read_sensor(sd, address, &value, 1);
496 pr_info("register 0x%x contains 0x%x\n", address, value);
497 }
498
499 pr_info("po1030 register state dump complete\n");
500
501 pr_info("Probing for which registers that are read/write\n");
502 for (address = 0; address < 0xff; address++) {
503 u8 old_value, ctrl_value;
504 u8 test_value[2] = {0xff, 0xff};
505
506 m5602_read_sensor(sd, address, &old_value, 1);
507 m5602_write_sensor(sd, address, test_value, 1);
508 m5602_read_sensor(sd, address, &ctrl_value, 1);
509
510 if (ctrl_value == test_value[0])
511 pr_info("register 0x%x is writeable\n", address);
512 else
513 pr_info("register 0x%x is read only\n", address);
514
515
516 m5602_write_sensor(sd, address, &old_value, 1);
517 }
518}
519
