Commit | Line | Data |
---|---|---|
343e89a7 BP |
1 | /* |
2 | * TI CAL camera interface driver | |
3 | * | |
4 | * Copyright (c) 2015 Texas Instruments Inc. | |
5 | * Benoit Parrot, <bparrot@ti.com> | |
6 | * | |
7 | * This program is free software; you can redistribute it and/or modify it | |
8 | * under the terms of the GNU General Public License version 2 as published by | |
9 | * the Free Software Foundation | |
10 | */ | |
11 | ||
12 | #include <linux/interrupt.h> | |
13 | #include <linux/io.h> | |
14 | #include <linux/ioctl.h> | |
15 | #include <linux/module.h> | |
16 | #include <linux/platform_device.h> | |
17 | #include <linux/delay.h> | |
18 | #include <linux/pm_runtime.h> | |
19 | #include <linux/slab.h> | |
20 | #include <linux/videodev2.h> | |
21 | #include <linux/of_device.h> | |
22 | #include <linux/of_graph.h> | |
23 | ||
24 | #include <media/v4l2-of.h> | |
25 | #include <media/v4l2-async.h> | |
26 | #include <media/v4l2-common.h> | |
27 | #include <media/v4l2-ctrls.h> | |
28 | #include <media/v4l2-device.h> | |
29 | #include <media/v4l2-event.h> | |
30 | #include <media/v4l2-ioctl.h> | |
31 | #include <media/v4l2-ctrls.h> | |
32 | #include <media/v4l2-fh.h> | |
33 | #include <media/v4l2-event.h> | |
34 | #include <media/v4l2-common.h> | |
35 | #include <media/videobuf2-core.h> | |
36 | #include <media/videobuf2-dma-contig.h> | |
37 | #include "cal_regs.h" | |
38 | ||
39 | #define CAL_MODULE_NAME "cal" | |
40 | ||
41 | #define MAX_WIDTH 1920 | |
42 | #define MAX_HEIGHT 1200 | |
43 | ||
44 | #define CAL_VERSION "0.1.0" | |
45 | ||
46 | MODULE_DESCRIPTION("TI CAL driver"); | |
47 | MODULE_AUTHOR("Benoit Parrot, <bparrot@ti.com>"); | |
48 | MODULE_LICENSE("GPL v2"); | |
49 | MODULE_VERSION(CAL_VERSION); | |
50 | ||
51 | static unsigned video_nr = -1; | |
52 | module_param(video_nr, uint, 0644); | |
53 | MODULE_PARM_DESC(video_nr, "videoX start number, -1 is autodetect"); | |
54 | ||
55 | static unsigned debug; | |
56 | module_param(debug, uint, 0644); | |
57 | MODULE_PARM_DESC(debug, "activates debug info"); | |
58 | ||
59 | /* timeperframe: min/max and default */ | |
60 | static const struct v4l2_fract | |
61 | tpf_default = {.numerator = 1001, .denominator = 30000}; | |
62 | ||
63 | #define cal_dbg(level, caldev, fmt, arg...) \ | |
64 | v4l2_dbg(level, debug, &caldev->v4l2_dev, fmt, ##arg) | |
65 | #define cal_info(caldev, fmt, arg...) \ | |
66 | v4l2_info(&caldev->v4l2_dev, fmt, ##arg) | |
67 | #define cal_err(caldev, fmt, arg...) \ | |
68 | v4l2_err(&caldev->v4l2_dev, fmt, ##arg) | |
69 | ||
70 | #define ctx_dbg(level, ctx, fmt, arg...) \ | |
71 | v4l2_dbg(level, debug, &ctx->v4l2_dev, fmt, ##arg) | |
72 | #define ctx_info(ctx, fmt, arg...) \ | |
73 | v4l2_info(&ctx->v4l2_dev, fmt, ##arg) | |
74 | #define ctx_err(ctx, fmt, arg...) \ | |
75 | v4l2_err(&ctx->v4l2_dev, fmt, ##arg) | |
76 | ||
77 | #define CAL_NUM_INPUT 1 | |
78 | #define CAL_NUM_CONTEXT 2 | |
79 | ||
80 | #define bytes_per_line(pixel, bpp) (ALIGN(pixel * bpp, 16)) | |
81 | ||
82 | #define reg_read(dev, offset) ioread32(dev->base + offset) | |
83 | #define reg_write(dev, offset, val) iowrite32(val, dev->base + offset) | |
84 | ||
85 | #define reg_read_field(dev, offset, mask) get_field(reg_read(dev, offset), \ | |
86 | mask) | |
87 | #define reg_write_field(dev, offset, field, mask) { \ | |
88 | u32 val = reg_read(dev, offset); \ | |
89 | set_field(&val, field, mask); \ | |
90 | reg_write(dev, offset, val); } | |
91 | ||
92 | /* ------------------------------------------------------------------ | |
93 | * Basic structures | |
94 | * ------------------------------------------------------------------ | |
95 | */ | |
96 | ||
97 | struct cal_fmt { | |
98 | u32 fourcc; | |
99 | u32 code; | |
100 | u8 depth; | |
101 | }; | |
102 | ||
103 | static struct cal_fmt cal_formats[] = { | |
104 | { | |
105 | .fourcc = V4L2_PIX_FMT_YUYV, | |
106 | .code = MEDIA_BUS_FMT_YUYV8_2X8, | |
107 | .depth = 16, | |
108 | }, { | |
109 | .fourcc = V4L2_PIX_FMT_UYVY, | |
110 | .code = MEDIA_BUS_FMT_UYVY8_2X8, | |
111 | .depth = 16, | |
112 | }, { | |
113 | .fourcc = V4L2_PIX_FMT_YVYU, | |
114 | .code = MEDIA_BUS_FMT_YVYU8_2X8, | |
115 | .depth = 16, | |
116 | }, { | |
117 | .fourcc = V4L2_PIX_FMT_VYUY, | |
118 | .code = MEDIA_BUS_FMT_VYUY8_2X8, | |
119 | .depth = 16, | |
120 | }, { | |
121 | .fourcc = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */ | |
122 | .code = MEDIA_BUS_FMT_RGB565_2X8_LE, | |
123 | .depth = 16, | |
124 | }, { | |
125 | .fourcc = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */ | |
126 | .code = MEDIA_BUS_FMT_RGB565_2X8_BE, | |
127 | .depth = 16, | |
128 | }, { | |
129 | .fourcc = V4L2_PIX_FMT_RGB555, /* gggbbbbb arrrrrgg */ | |
130 | .code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_LE, | |
131 | .depth = 16, | |
132 | }, { | |
133 | .fourcc = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */ | |
134 | .code = MEDIA_BUS_FMT_RGB555_2X8_PADHI_BE, | |
135 | .depth = 16, | |
136 | }, { | |
137 | .fourcc = V4L2_PIX_FMT_RGB24, /* rgb */ | |
138 | .code = MEDIA_BUS_FMT_RGB888_2X12_LE, | |
139 | .depth = 24, | |
140 | }, { | |
141 | .fourcc = V4L2_PIX_FMT_BGR24, /* bgr */ | |
142 | .code = MEDIA_BUS_FMT_RGB888_2X12_BE, | |
143 | .depth = 24, | |
144 | }, { | |
145 | .fourcc = V4L2_PIX_FMT_RGB32, /* argb */ | |
146 | .code = MEDIA_BUS_FMT_ARGB8888_1X32, | |
147 | .depth = 32, | |
148 | }, { | |
149 | .fourcc = V4L2_PIX_FMT_SBGGR8, | |
150 | .code = MEDIA_BUS_FMT_SBGGR8_1X8, | |
151 | .depth = 8, | |
152 | }, { | |
153 | .fourcc = V4L2_PIX_FMT_SGBRG8, | |
154 | .code = MEDIA_BUS_FMT_SGBRG8_1X8, | |
155 | .depth = 8, | |
156 | }, { | |
157 | .fourcc = V4L2_PIX_FMT_SGRBG8, | |
158 | .code = MEDIA_BUS_FMT_SGRBG8_1X8, | |
159 | .depth = 8, | |
160 | }, { | |
161 | .fourcc = V4L2_PIX_FMT_SRGGB8, | |
162 | .code = MEDIA_BUS_FMT_SRGGB8_1X8, | |
163 | .depth = 8, | |
164 | }, { | |
165 | .fourcc = V4L2_PIX_FMT_SBGGR10, | |
166 | .code = MEDIA_BUS_FMT_SBGGR10_1X10, | |
167 | .depth = 16, | |
168 | }, { | |
169 | .fourcc = V4L2_PIX_FMT_SGBRG10, | |
170 | .code = MEDIA_BUS_FMT_SGBRG10_1X10, | |
171 | .depth = 16, | |
172 | }, { | |
173 | .fourcc = V4L2_PIX_FMT_SGRBG10, | |
174 | .code = MEDIA_BUS_FMT_SGRBG10_1X10, | |
175 | .depth = 16, | |
176 | }, { | |
177 | .fourcc = V4L2_PIX_FMT_SRGGB10, | |
178 | .code = MEDIA_BUS_FMT_SRGGB10_1X10, | |
179 | .depth = 16, | |
180 | }, { | |
181 | .fourcc = V4L2_PIX_FMT_SBGGR12, | |
182 | .code = MEDIA_BUS_FMT_SBGGR12_1X12, | |
183 | .depth = 16, | |
184 | }, { | |
185 | .fourcc = V4L2_PIX_FMT_SGBRG12, | |
186 | .code = MEDIA_BUS_FMT_SGBRG12_1X12, | |
187 | .depth = 16, | |
188 | }, { | |
189 | .fourcc = V4L2_PIX_FMT_SGRBG12, | |
190 | .code = MEDIA_BUS_FMT_SGRBG12_1X12, | |
191 | .depth = 16, | |
192 | }, { | |
193 | .fourcc = V4L2_PIX_FMT_SRGGB12, | |
194 | .code = MEDIA_BUS_FMT_SRGGB12_1X12, | |
195 | .depth = 16, | |
196 | }, | |
197 | }; | |
198 | ||
199 | /* Print Four-character-code (FOURCC) */ | |
200 | static char *fourcc_to_str(u32 fmt) | |
201 | { | |
202 | static char code[5]; | |
203 | ||
204 | code[0] = (unsigned char)(fmt & 0xff); | |
205 | code[1] = (unsigned char)((fmt >> 8) & 0xff); | |
206 | code[2] = (unsigned char)((fmt >> 16) & 0xff); | |
207 | code[3] = (unsigned char)((fmt >> 24) & 0xff); | |
208 | code[4] = '\0'; | |
209 | ||
210 | return code; | |
211 | } | |
212 | ||
213 | /* buffer for one video frame */ | |
214 | struct cal_buffer { | |
215 | /* common v4l buffer stuff -- must be first */ | |
216 | struct vb2_v4l2_buffer vb; | |
217 | struct list_head list; | |
218 | const struct cal_fmt *fmt; | |
219 | }; | |
220 | ||
221 | struct cal_dmaqueue { | |
222 | struct list_head active; | |
223 | ||
224 | /* Counters to control fps rate */ | |
225 | int frame; | |
226 | int ini_jiffies; | |
227 | }; | |
228 | ||
229 | struct cm_data { | |
230 | void __iomem *base; | |
231 | struct resource *res; | |
232 | ||
233 | unsigned int camerrx_control; | |
234 | ||
235 | struct platform_device *pdev; | |
236 | }; | |
237 | ||
238 | struct cc_data { | |
239 | void __iomem *base; | |
240 | struct resource *res; | |
241 | ||
242 | struct platform_device *pdev; | |
243 | }; | |
244 | ||
245 | /* | |
246 | * there is one cal_dev structure in the driver, it is shared by | |
247 | * all instances. | |
248 | */ | |
249 | struct cal_dev { | |
250 | int irq; | |
251 | void __iomem *base; | |
252 | struct resource *res; | |
253 | struct platform_device *pdev; | |
254 | struct v4l2_device v4l2_dev; | |
255 | ||
256 | /* Control Module handle */ | |
257 | struct cm_data *cm; | |
258 | /* Camera Core Module handle */ | |
259 | struct cc_data *cc[CAL_NUM_CSI2_PORTS]; | |
260 | ||
261 | struct cal_ctx *ctx[CAL_NUM_CONTEXT]; | |
262 | }; | |
263 | ||
264 | /* | |
265 | * There is one cal_ctx structure for each camera core context. | |
266 | */ | |
267 | struct cal_ctx { | |
268 | struct v4l2_device v4l2_dev; | |
269 | struct v4l2_ctrl_handler ctrl_handler; | |
270 | struct video_device vdev; | |
271 | struct v4l2_async_notifier notifier; | |
272 | struct v4l2_subdev *sensor; | |
273 | struct v4l2_of_endpoint endpoint; | |
274 | ||
275 | struct v4l2_async_subdev asd; | |
276 | struct v4l2_async_subdev *asd_list[1]; | |
277 | ||
278 | struct v4l2_fh fh; | |
279 | struct cal_dev *dev; | |
280 | struct cc_data *cc; | |
281 | ||
282 | /* v4l2_ioctl mutex */ | |
283 | struct mutex mutex; | |
284 | /* v4l2 buffers lock */ | |
285 | spinlock_t slock; | |
286 | ||
287 | /* Several counters */ | |
288 | unsigned long jiffies; | |
289 | ||
343e89a7 BP |
290 | struct cal_dmaqueue vidq; |
291 | ||
292 | /* Input Number */ | |
293 | int input; | |
294 | ||
295 | /* video capture */ | |
296 | const struct cal_fmt *fmt; | |
297 | /* Used to store current pixel format */ | |
298 | struct v4l2_format v_fmt; | |
299 | /* Used to store current mbus frame format */ | |
300 | struct v4l2_mbus_framefmt m_fmt; | |
301 | ||
302 | /* Current subdev enumerated format */ | |
303 | struct cal_fmt *active_fmt[ARRAY_SIZE(cal_formats)]; | |
304 | int num_active_fmt; | |
305 | ||
306 | struct v4l2_fract timeperframe; | |
307 | unsigned int sequence; | |
308 | unsigned int external_rate; | |
309 | struct vb2_queue vb_vidq; | |
310 | unsigned int seq_count; | |
311 | unsigned int csi2_port; | |
312 | unsigned int virtual_channel; | |
313 | ||
314 | /* Pointer pointing to current v4l2_buffer */ | |
315 | struct cal_buffer *cur_frm; | |
316 | /* Pointer pointing to next v4l2_buffer */ | |
317 | struct cal_buffer *next_frm; | |
318 | }; | |
319 | ||
320 | static const struct cal_fmt *find_format_by_pix(struct cal_ctx *ctx, | |
321 | u32 pixelformat) | |
322 | { | |
323 | const struct cal_fmt *fmt; | |
324 | unsigned int k; | |
325 | ||
326 | for (k = 0; k < ctx->num_active_fmt; k++) { | |
327 | fmt = ctx->active_fmt[k]; | |
328 | if (fmt->fourcc == pixelformat) | |
329 | return fmt; | |
330 | } | |
331 | ||
332 | return NULL; | |
333 | } | |
334 | ||
335 | static const struct cal_fmt *find_format_by_code(struct cal_ctx *ctx, | |
336 | u32 code) | |
337 | { | |
338 | const struct cal_fmt *fmt; | |
339 | unsigned int k; | |
340 | ||
341 | for (k = 0; k < ctx->num_active_fmt; k++) { | |
342 | fmt = ctx->active_fmt[k]; | |
343 | if (fmt->code == code) | |
344 | return fmt; | |
345 | } | |
346 | ||
347 | return NULL; | |
348 | } | |
349 | ||
350 | static inline struct cal_ctx *notifier_to_ctx(struct v4l2_async_notifier *n) | |
351 | { | |
352 | return container_of(n, struct cal_ctx, notifier); | |
353 | } | |
354 | ||
355 | static inline int get_field(u32 value, u32 mask) | |
356 | { | |
357 | return (value & mask) >> __ffs(mask); | |
358 | } | |
359 | ||
360 | static inline void set_field(u32 *valp, u32 field, u32 mask) | |
361 | { | |
362 | u32 val = *valp; | |
363 | ||
364 | val &= ~mask; | |
365 | val |= (field << __ffs(mask)) & mask; | |
366 | *valp = val; | |
367 | } | |
368 | ||
369 | /* | |
370 | * Control Module block access | |
371 | */ | |
372 | static struct cm_data *cm_create(struct cal_dev *dev) | |
373 | { | |
374 | struct platform_device *pdev = dev->pdev; | |
375 | struct cm_data *cm; | |
376 | ||
377 | cm = devm_kzalloc(&pdev->dev, sizeof(*cm), GFP_KERNEL); | |
378 | if (!cm) | |
379 | return ERR_PTR(-ENOMEM); | |
380 | ||
381 | cm->res = platform_get_resource_byname(pdev, IORESOURCE_MEM, | |
382 | "camerrx_control"); | |
383 | cm->base = devm_ioremap_resource(&pdev->dev, cm->res); | |
384 | if (IS_ERR(cm->base)) { | |
385 | cal_err(dev, "failed to ioremap\n"); | |
96621112 | 386 | return ERR_CAST(cm->base); |
343e89a7 BP |
387 | } |
388 | ||
389 | cal_dbg(1, dev, "ioresource %s at %pa - %pa\n", | |
390 | cm->res->name, &cm->res->start, &cm->res->end); | |
391 | ||
392 | return cm; | |
393 | } | |
394 | ||
395 | static void camerarx_phy_enable(struct cal_ctx *ctx) | |
396 | { | |
397 | u32 val; | |
398 | ||
399 | if (!ctx->dev->cm->base) { | |
400 | ctx_err(ctx, "cm not mapped\n"); | |
401 | return; | |
402 | } | |
403 | ||
404 | val = reg_read(ctx->dev->cm, CM_CTRL_CORE_CAMERRX_CONTROL); | |
405 | if (ctx->csi2_port == 1) { | |
406 | set_field(&val, 1, CM_CAMERRX_CTRL_CSI0_CTRLCLKEN_MASK); | |
407 | set_field(&val, 0, CM_CAMERRX_CTRL_CSI0_CAMMODE_MASK); | |
408 | /* enable all lanes by default */ | |
409 | set_field(&val, 0xf, CM_CAMERRX_CTRL_CSI0_LANEENABLE_MASK); | |
410 | set_field(&val, 1, CM_CAMERRX_CTRL_CSI0_MODE_MASK); | |
411 | } else if (ctx->csi2_port == 2) { | |
412 | set_field(&val, 1, CM_CAMERRX_CTRL_CSI1_CTRLCLKEN_MASK); | |
413 | set_field(&val, 0, CM_CAMERRX_CTRL_CSI1_CAMMODE_MASK); | |
414 | /* enable all lanes by default */ | |
415 | set_field(&val, 0x3, CM_CAMERRX_CTRL_CSI1_LANEENABLE_MASK); | |
416 | set_field(&val, 1, CM_CAMERRX_CTRL_CSI1_MODE_MASK); | |
417 | } | |
418 | reg_write(ctx->dev->cm, CM_CTRL_CORE_CAMERRX_CONTROL, val); | |
419 | } | |
420 | ||
421 | static void camerarx_phy_disable(struct cal_ctx *ctx) | |
422 | { | |
423 | u32 val; | |
424 | ||
425 | if (!ctx->dev->cm->base) { | |
426 | ctx_err(ctx, "cm not mapped\n"); | |
427 | return; | |
428 | } | |
429 | ||
430 | val = reg_read(ctx->dev->cm, CM_CTRL_CORE_CAMERRX_CONTROL); | |
431 | if (ctx->csi2_port == 1) | |
432 | set_field(&val, 0x0, CM_CAMERRX_CTRL_CSI0_CTRLCLKEN_MASK); | |
433 | else if (ctx->csi2_port == 2) | |
434 | set_field(&val, 0x0, CM_CAMERRX_CTRL_CSI1_CTRLCLKEN_MASK); | |
435 | reg_write(ctx->dev->cm, CM_CTRL_CORE_CAMERRX_CONTROL, val); | |
436 | } | |
437 | ||
438 | /* | |
439 | * Camera Instance access block | |
440 | */ | |
441 | static struct cc_data *cc_create(struct cal_dev *dev, unsigned int core) | |
442 | { | |
443 | struct platform_device *pdev = dev->pdev; | |
444 | struct cc_data *cc; | |
445 | ||
446 | cc = devm_kzalloc(&pdev->dev, sizeof(*cc), GFP_KERNEL); | |
447 | if (!cc) | |
448 | return ERR_PTR(-ENOMEM); | |
449 | ||
450 | cc->res = platform_get_resource_byname(pdev, | |
451 | IORESOURCE_MEM, | |
452 | (core == 0) ? | |
453 | "cal_rx_core0" : | |
454 | "cal_rx_core1"); | |
455 | cc->base = devm_ioremap_resource(&pdev->dev, cc->res); | |
456 | if (IS_ERR(cc->base)) { | |
457 | cal_err(dev, "failed to ioremap\n"); | |
96621112 | 458 | return ERR_CAST(cc->base); |
343e89a7 BP |
459 | } |
460 | ||
461 | cal_dbg(1, dev, "ioresource %s at %pa - %pa\n", | |
462 | cc->res->name, &cc->res->start, &cc->res->end); | |
463 | ||
464 | return cc; | |
465 | } | |
466 | ||
467 | /* | |
468 | * Get Revision and HW info | |
469 | */ | |
470 | static void cal_get_hwinfo(struct cal_dev *dev) | |
471 | { | |
472 | u32 revision = 0; | |
473 | u32 hwinfo = 0; | |
474 | ||
475 | revision = reg_read(dev, CAL_HL_REVISION); | |
476 | cal_dbg(3, dev, "CAL_HL_REVISION = 0x%08x (expecting 0x40000200)\n", | |
477 | revision); | |
478 | ||
479 | hwinfo = reg_read(dev, CAL_HL_HWINFO); | |
480 | cal_dbg(3, dev, "CAL_HL_HWINFO = 0x%08x (expecting 0xA3C90469)\n", | |
481 | hwinfo); | |
482 | } | |
483 | ||
484 | static inline int cal_runtime_get(struct cal_dev *dev) | |
485 | { | |
486 | int r; | |
487 | ||
488 | r = pm_runtime_get_sync(&dev->pdev->dev); | |
489 | ||
490 | return r; | |
491 | } | |
492 | ||
493 | static inline void cal_runtime_put(struct cal_dev *dev) | |
494 | { | |
495 | pm_runtime_put_sync(&dev->pdev->dev); | |
496 | } | |
497 | ||
498 | static void cal_quickdump_regs(struct cal_dev *dev) | |
499 | { | |
500 | cal_info(dev, "CAL Registers @ 0x%pa:\n", &dev->res->start); | |
501 | print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 4, | |
96621112 BP |
502 | (__force const void *)dev->base, |
503 | resource_size(dev->res), false); | |
343e89a7 BP |
504 | |
505 | if (dev->ctx[0]) { | |
506 | cal_info(dev, "CSI2 Core 0 Registers @ %pa:\n", | |
507 | &dev->ctx[0]->cc->res->start); | |
508 | print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 4, | |
96621112 | 509 | (__force const void *)dev->ctx[0]->cc->base, |
343e89a7 BP |
510 | resource_size(dev->ctx[0]->cc->res), |
511 | false); | |
512 | } | |
513 | ||
514 | if (dev->ctx[1]) { | |
515 | cal_info(dev, "CSI2 Core 1 Registers @ %pa:\n", | |
516 | &dev->ctx[1]->cc->res->start); | |
517 | print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 4, | |
96621112 | 518 | (__force const void *)dev->ctx[1]->cc->base, |
343e89a7 BP |
519 | resource_size(dev->ctx[1]->cc->res), |
520 | false); | |
521 | } | |
522 | ||
523 | cal_info(dev, "CAMERRX_Control Registers @ %pa:\n", | |
524 | &dev->cm->res->start); | |
525 | print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 16, 4, | |
96621112 | 526 | (__force const void *)dev->cm->base, |
343e89a7 BP |
527 | resource_size(dev->cm->res), false); |
528 | } | |
529 | ||
530 | /* | |
531 | * Enable the expected IRQ sources | |
532 | */ | |
533 | static void enable_irqs(struct cal_ctx *ctx) | |
534 | { | |
535 | /* Enable IRQ_WDMA_END 0/1 */ | |
536 | reg_write_field(ctx->dev, | |
537 | CAL_HL_IRQENABLE_SET(2), | |
538 | CAL_HL_IRQ_ENABLE, | |
539 | CAL_HL_IRQ_MASK(ctx->csi2_port)); | |
540 | /* Enable IRQ_WDMA_START 0/1 */ | |
541 | reg_write_field(ctx->dev, | |
542 | CAL_HL_IRQENABLE_SET(3), | |
543 | CAL_HL_IRQ_ENABLE, | |
544 | CAL_HL_IRQ_MASK(ctx->csi2_port)); | |
545 | /* Todo: Add VC_IRQ and CSI2_COMPLEXIO_IRQ handling */ | |
546 | reg_write(ctx->dev, CAL_CSI2_VC_IRQENABLE(1), 0xFF000000); | |
547 | } | |
548 | ||
549 | static void disable_irqs(struct cal_ctx *ctx) | |
550 | { | |
551 | /* Disable IRQ_WDMA_END 0/1 */ | |
552 | reg_write_field(ctx->dev, | |
553 | CAL_HL_IRQENABLE_CLR(2), | |
554 | CAL_HL_IRQ_CLEAR, | |
555 | CAL_HL_IRQ_MASK(ctx->csi2_port)); | |
556 | /* Disable IRQ_WDMA_START 0/1 */ | |
557 | reg_write_field(ctx->dev, | |
558 | CAL_HL_IRQENABLE_CLR(3), | |
559 | CAL_HL_IRQ_CLEAR, | |
560 | CAL_HL_IRQ_MASK(ctx->csi2_port)); | |
561 | /* Todo: Add VC_IRQ and CSI2_COMPLEXIO_IRQ handling */ | |
562 | reg_write(ctx->dev, CAL_CSI2_VC_IRQENABLE(1), 0); | |
563 | } | |
564 | ||
565 | static void csi2_init(struct cal_ctx *ctx) | |
566 | { | |
567 | int i; | |
568 | u32 val; | |
569 | ||
570 | val = reg_read(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port)); | |
571 | set_field(&val, CAL_GEN_ENABLE, | |
572 | CAL_CSI2_TIMING_FORCE_RX_MODE_IO1_MASK); | |
573 | set_field(&val, CAL_GEN_ENABLE, | |
574 | CAL_CSI2_TIMING_STOP_STATE_X16_IO1_MASK); | |
575 | set_field(&val, CAL_GEN_DISABLE, | |
576 | CAL_CSI2_TIMING_STOP_STATE_X4_IO1_MASK); | |
577 | set_field(&val, 407, CAL_CSI2_TIMING_STOP_STATE_COUNTER_IO1_MASK); | |
578 | reg_write(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port), val); | |
579 | ctx_dbg(3, ctx, "CAL_CSI2_TIMING(%d) = 0x%08x\n", ctx->csi2_port, | |
580 | reg_read(ctx->dev, CAL_CSI2_TIMING(ctx->csi2_port))); | |
581 | ||
582 | val = reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port)); | |
583 | set_field(&val, CAL_CSI2_COMPLEXIO_CFG_RESET_CTRL_OPERATIONAL, | |
584 | CAL_CSI2_COMPLEXIO_CFG_RESET_CTRL_MASK); | |
585 | set_field(&val, CAL_CSI2_COMPLEXIO_CFG_PWR_CMD_STATE_ON, | |
586 | CAL_CSI2_COMPLEXIO_CFG_PWR_CMD_MASK); | |
587 | reg_write(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port), val); | |
588 | for (i = 0; i < 10; i++) { | |
589 | if (reg_read_field(ctx->dev, | |
590 | CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port), | |
591 | CAL_CSI2_COMPLEXIO_CFG_PWR_STATUS_MASK) == | |
592 | CAL_CSI2_COMPLEXIO_CFG_PWR_STATUS_STATE_ON) | |
593 | break; | |
594 | usleep_range(1000, 1100); | |
595 | } | |
596 | ctx_dbg(3, ctx, "CAL_CSI2_COMPLEXIO_CFG(%d) = 0x%08x\n", ctx->csi2_port, | |
597 | reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port))); | |
598 | ||
599 | val = reg_read(ctx->dev, CAL_CTRL); | |
600 | set_field(&val, CAL_CTRL_BURSTSIZE_BURST128, CAL_CTRL_BURSTSIZE_MASK); | |
601 | set_field(&val, 0xF, CAL_CTRL_TAGCNT_MASK); | |
602 | set_field(&val, CAL_CTRL_POSTED_WRITES_NONPOSTED, | |
603 | CAL_CTRL_POSTED_WRITES_MASK); | |
604 | set_field(&val, 0xFF, CAL_CTRL_MFLAGL_MASK); | |
605 | set_field(&val, 0xFF, CAL_CTRL_MFLAGH_MASK); | |
606 | reg_write(ctx->dev, CAL_CTRL, val); | |
607 | ctx_dbg(3, ctx, "CAL_CTRL = 0x%08x\n", reg_read(ctx->dev, CAL_CTRL)); | |
608 | } | |
609 | ||
610 | static void csi2_lane_config(struct cal_ctx *ctx) | |
611 | { | |
612 | u32 val = reg_read(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port)); | |
613 | u32 lane_mask = CAL_CSI2_COMPLEXIO_CFG_CLOCK_POSITION_MASK; | |
614 | u32 polarity_mask = CAL_CSI2_COMPLEXIO_CFG_CLOCK_POL_MASK; | |
615 | struct v4l2_of_bus_mipi_csi2 *mipi_csi2 = &ctx->endpoint.bus.mipi_csi2; | |
616 | int lane; | |
617 | ||
618 | set_field(&val, mipi_csi2->clock_lane + 1, lane_mask); | |
619 | set_field(&val, mipi_csi2->lane_polarities[0], polarity_mask); | |
620 | for (lane = 0; lane < mipi_csi2->num_data_lanes; lane++) { | |
621 | /* | |
622 | * Every lane are one nibble apart starting with the | |
623 | * clock followed by the data lanes so shift masks by 4. | |
624 | */ | |
625 | lane_mask <<= 4; | |
626 | polarity_mask <<= 4; | |
627 | set_field(&val, mipi_csi2->data_lanes[lane] + 1, lane_mask); | |
628 | set_field(&val, mipi_csi2->lane_polarities[lane + 1], | |
629 | polarity_mask); | |
630 | } | |
631 | ||
632 | reg_write(ctx->dev, CAL_CSI2_COMPLEXIO_CFG(ctx->csi2_port), val); | |
633 | ctx_dbg(3, ctx, "CAL_CSI2_COMPLEXIO_CFG(%d) = 0x%08x\n", | |
634 | ctx->csi2_port, val); | |
635 | } | |
636 | ||
637 | static void csi2_ppi_enable(struct cal_ctx *ctx) | |
638 | { | |
639 | reg_write_field(ctx->dev, CAL_CSI2_PPI_CTRL(ctx->csi2_port), | |
640 | CAL_GEN_ENABLE, CAL_CSI2_PPI_CTRL_IF_EN_MASK); | |
641 | } | |
642 | ||
643 | static void csi2_ppi_disable(struct cal_ctx *ctx) | |
644 | { | |
645 | reg_write_field(ctx->dev, CAL_CSI2_PPI_CTRL(ctx->csi2_port), | |
646 | CAL_GEN_DISABLE, CAL_CSI2_PPI_CTRL_IF_EN_MASK); | |
647 | } | |
648 | ||
649 | static void csi2_ctx_config(struct cal_ctx *ctx) | |
650 | { | |
651 | u32 val; | |
652 | ||
653 | val = reg_read(ctx->dev, CAL_CSI2_CTX0(ctx->csi2_port)); | |
654 | set_field(&val, ctx->csi2_port, CAL_CSI2_CTX_CPORT_MASK); | |
655 | /* | |
656 | * DT type: MIPI CSI-2 Specs | |
657 | * 0x1: All - DT filter is disabled | |
658 | * 0x24: RGB888 1 pixel = 3 bytes | |
659 | * 0x2B: RAW10 4 pixels = 5 bytes | |
660 | * 0x2A: RAW8 1 pixel = 1 byte | |
661 | * 0x1E: YUV422 2 pixels = 4 bytes | |
662 | */ | |
663 | set_field(&val, 0x1, CAL_CSI2_CTX_DT_MASK); | |
664 | /* Virtual Channel from the CSI2 sensor usually 0! */ | |
665 | set_field(&val, ctx->virtual_channel, CAL_CSI2_CTX_VC_MASK); | |
666 | /* NUM_LINES_PER_FRAME => 0 means auto detect */ | |
667 | set_field(&val, 0, CAL_CSI2_CTX_LINES_MASK); | |
668 | set_field(&val, CAL_CSI2_CTX_ATT_PIX, CAL_CSI2_CTX_ATT_MASK); | |
669 | set_field(&val, CAL_CSI2_CTX_PACK_MODE_LINE, | |
670 | CAL_CSI2_CTX_PACK_MODE_MASK); | |
671 | reg_write(ctx->dev, CAL_CSI2_CTX0(ctx->csi2_port), val); | |
672 | ctx_dbg(3, ctx, "CAL_CSI2_CTX0(%d) = 0x%08x\n", ctx->csi2_port, | |
673 | reg_read(ctx->dev, CAL_CSI2_CTX0(ctx->csi2_port))); | |
674 | } | |
675 | ||
676 | static void pix_proc_config(struct cal_ctx *ctx) | |
677 | { | |
678 | u32 val; | |
679 | ||
680 | val = reg_read(ctx->dev, CAL_PIX_PROC(ctx->csi2_port)); | |
681 | set_field(&val, CAL_PIX_PROC_EXTRACT_B8, CAL_PIX_PROC_EXTRACT_MASK); | |
682 | set_field(&val, CAL_PIX_PROC_DPCMD_BYPASS, CAL_PIX_PROC_DPCMD_MASK); | |
683 | set_field(&val, CAL_PIX_PROC_DPCME_BYPASS, CAL_PIX_PROC_DPCME_MASK); | |
684 | set_field(&val, CAL_PIX_PROC_PACK_B8, CAL_PIX_PROC_PACK_MASK); | |
685 | set_field(&val, ctx->csi2_port, CAL_PIX_PROC_CPORT_MASK); | |
686 | set_field(&val, CAL_GEN_ENABLE, CAL_PIX_PROC_EN_MASK); | |
687 | reg_write(ctx->dev, CAL_PIX_PROC(ctx->csi2_port), val); | |
688 | ctx_dbg(3, ctx, "CAL_PIX_PROC(%d) = 0x%08x\n", ctx->csi2_port, | |
689 | reg_read(ctx->dev, CAL_PIX_PROC(ctx->csi2_port))); | |
690 | } | |
691 | ||
692 | static void cal_wr_dma_config(struct cal_ctx *ctx, | |
693 | unsigned int width) | |
694 | { | |
695 | u32 val; | |
696 | ||
697 | val = reg_read(ctx->dev, CAL_WR_DMA_CTRL(ctx->csi2_port)); | |
698 | set_field(&val, ctx->csi2_port, CAL_WR_DMA_CTRL_CPORT_MASK); | |
699 | set_field(&val, CAL_WR_DMA_CTRL_DTAG_PIX_DAT, | |
700 | CAL_WR_DMA_CTRL_DTAG_MASK); | |
701 | set_field(&val, CAL_WR_DMA_CTRL_MODE_CONST, | |
702 | CAL_WR_DMA_CTRL_MODE_MASK); | |
703 | set_field(&val, CAL_WR_DMA_CTRL_PATTERN_LINEAR, | |
704 | CAL_WR_DMA_CTRL_PATTERN_MASK); | |
705 | set_field(&val, CAL_GEN_ENABLE, CAL_WR_DMA_CTRL_STALL_RD_MASK); | |
706 | reg_write(ctx->dev, CAL_WR_DMA_CTRL(ctx->csi2_port), val); | |
707 | ctx_dbg(3, ctx, "CAL_WR_DMA_CTRL(%d) = 0x%08x\n", ctx->csi2_port, | |
708 | reg_read(ctx->dev, CAL_WR_DMA_CTRL(ctx->csi2_port))); | |
709 | ||
710 | /* | |
711 | * width/16 not sure but giving it a whirl. | |
712 | * zero does not work right | |
713 | */ | |
714 | reg_write_field(ctx->dev, | |
715 | CAL_WR_DMA_OFST(ctx->csi2_port), | |
716 | (width / 16), | |
717 | CAL_WR_DMA_OFST_MASK); | |
718 | ctx_dbg(3, ctx, "CAL_WR_DMA_OFST(%d) = 0x%08x\n", ctx->csi2_port, | |
719 | reg_read(ctx->dev, CAL_WR_DMA_OFST(ctx->csi2_port))); | |
720 | ||
721 | val = reg_read(ctx->dev, CAL_WR_DMA_XSIZE(ctx->csi2_port)); | |
722 | /* 64 bit word means no skipping */ | |
723 | set_field(&val, 0, CAL_WR_DMA_XSIZE_XSKIP_MASK); | |
724 | /* | |
725 | * (width*8)/64 this should be size of an entire line | |
726 | * in 64bit word but 0 means all data until the end | |
727 | * is detected automagically | |
728 | */ | |
729 | set_field(&val, (width / 8), CAL_WR_DMA_XSIZE_MASK); | |
730 | reg_write(ctx->dev, CAL_WR_DMA_XSIZE(ctx->csi2_port), val); | |
731 | ctx_dbg(3, ctx, "CAL_WR_DMA_XSIZE(%d) = 0x%08x\n", ctx->csi2_port, | |
732 | reg_read(ctx->dev, CAL_WR_DMA_XSIZE(ctx->csi2_port))); | |
733 | } | |
734 | ||
735 | static void cal_wr_dma_addr(struct cal_ctx *ctx, unsigned int dmaaddr) | |
736 | { | |
737 | reg_write(ctx->dev, CAL_WR_DMA_ADDR(ctx->csi2_port), dmaaddr); | |
738 | } | |
739 | ||
740 | /* | |
741 | * TCLK values are OK at their reset values | |
742 | */ | |
743 | #define TCLK_TERM 0 | |
744 | #define TCLK_MISS 1 | |
745 | #define TCLK_SETTLE 14 | |
746 | #define THS_SETTLE 15 | |
747 | ||
748 | static void csi2_phy_config(struct cal_ctx *ctx) | |
749 | { | |
750 | unsigned int reg0, reg1; | |
751 | unsigned int ths_term, ths_settle; | |
752 | unsigned int ddrclkperiod_us; | |
753 | ||
754 | /* | |
755 | * THS_TERM: Programmed value = floor(20 ns/DDRClk period) - 2. | |
756 | */ | |
757 | ddrclkperiod_us = ctx->external_rate / 2000000; | |
758 | ddrclkperiod_us = 1000000 / ddrclkperiod_us; | |
759 | ctx_dbg(1, ctx, "ddrclkperiod_us: %d\n", ddrclkperiod_us); | |
760 | ||
761 | ths_term = 20000 / ddrclkperiod_us; | |
762 | ths_term = (ths_term >= 2) ? ths_term - 2 : ths_term; | |
763 | ctx_dbg(1, ctx, "ths_term: %d (0x%02x)\n", ths_term, ths_term); | |
764 | ||
765 | /* | |
766 | * THS_SETTLE: Programmed value = floor(176.3 ns/CtrlClk period) - 1. | |
767 | * Since CtrlClk is fixed at 96Mhz then we get | |
768 | * ths_settle = floor(176.3 / 10.416) - 1 = 15 | |
769 | * If we ever switch to a dynamic clock then this code might be useful | |
770 | * | |
771 | * unsigned int ctrlclkperiod_us; | |
772 | * ctrlclkperiod_us = 96000000 / 1000000; | |
773 | * ctrlclkperiod_us = 1000000 / ctrlclkperiod_us; | |
774 | * ctx_dbg(1, ctx, "ctrlclkperiod_us: %d\n", ctrlclkperiod_us); | |
775 | ||
776 | * ths_settle = 176300 / ctrlclkperiod_us; | |
777 | * ths_settle = (ths_settle > 1) ? ths_settle - 1 : ths_settle; | |
778 | */ | |
779 | ||
780 | ths_settle = THS_SETTLE; | |
781 | ctx_dbg(1, ctx, "ths_settle: %d (0x%02x)\n", ths_settle, ths_settle); | |
782 | ||
783 | reg0 = reg_read(ctx->cc, CAL_CSI2_PHY_REG0); | |
784 | set_field(®0, CAL_CSI2_PHY_REG0_HSCLOCKCONFIG_DISABLE, | |
785 | CAL_CSI2_PHY_REG0_HSCLOCKCONFIG_MASK); | |
786 | set_field(®0, ths_term, CAL_CSI2_PHY_REG0_THS_TERM_MASK); | |
787 | set_field(®0, ths_settle, CAL_CSI2_PHY_REG0_THS_SETTLE_MASK); | |
788 | ||
789 | ctx_dbg(1, ctx, "CSI2_%d_REG0 = 0x%08x\n", (ctx->csi2_port - 1), reg0); | |
790 | reg_write(ctx->cc, CAL_CSI2_PHY_REG0, reg0); | |
791 | ||
792 | reg1 = reg_read(ctx->cc, CAL_CSI2_PHY_REG1); | |
793 | set_field(®1, TCLK_TERM, CAL_CSI2_PHY_REG1_TCLK_TERM_MASK); | |
794 | set_field(®1, 0xb8, CAL_CSI2_PHY_REG1_DPHY_HS_SYNC_PATTERN_MASK); | |
795 | set_field(®1, TCLK_MISS, CAL_CSI2_PHY_REG1_CTRLCLK_DIV_FACTOR_MASK); | |
796 | set_field(®1, TCLK_SETTLE, CAL_CSI2_PHY_REG1_TCLK_SETTLE_MASK); | |
797 | ||
798 | ctx_dbg(1, ctx, "CSI2_%d_REG1 = 0x%08x\n", (ctx->csi2_port - 1), reg1); | |
799 | reg_write(ctx->cc, CAL_CSI2_PHY_REG1, reg1); | |
800 | } | |
801 | ||
802 | static int cal_get_external_info(struct cal_ctx *ctx) | |
803 | { | |
804 | struct v4l2_ctrl *ctrl; | |
805 | ||
2ddf22ee BP |
806 | if (!ctx->sensor) |
807 | return -ENODEV; | |
808 | ||
343e89a7 BP |
809 | ctrl = v4l2_ctrl_find(ctx->sensor->ctrl_handler, V4L2_CID_PIXEL_RATE); |
810 | if (!ctrl) { | |
811 | ctx_err(ctx, "no pixel rate control in subdev: %s\n", | |
812 | ctx->sensor->name); | |
813 | return -EPIPE; | |
814 | } | |
815 | ||
816 | ctx->external_rate = v4l2_ctrl_g_ctrl_int64(ctrl); | |
817 | ctx_dbg(3, ctx, "sensor Pixel Rate: %d\n", ctx->external_rate); | |
818 | ||
819 | return 0; | |
820 | } | |
821 | ||
822 | static inline void cal_schedule_next_buffer(struct cal_ctx *ctx) | |
823 | { | |
824 | struct cal_dmaqueue *dma_q = &ctx->vidq; | |
825 | struct cal_buffer *buf; | |
826 | unsigned long addr; | |
827 | ||
828 | buf = list_entry(dma_q->active.next, struct cal_buffer, list); | |
829 | ctx->next_frm = buf; | |
830 | list_del(&buf->list); | |
831 | ||
832 | addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0); | |
833 | cal_wr_dma_addr(ctx, addr); | |
834 | } | |
835 | ||
836 | static inline void cal_process_buffer_complete(struct cal_ctx *ctx) | |
837 | { | |
838 | ctx->cur_frm->vb.vb2_buf.timestamp = ktime_get_ns(); | |
839 | ctx->cur_frm->vb.field = ctx->m_fmt.field; | |
840 | ctx->cur_frm->vb.sequence = ctx->sequence++; | |
841 | ||
842 | vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_DONE); | |
843 | ctx->cur_frm = ctx->next_frm; | |
844 | } | |
845 | ||
846 | #define isvcirqset(irq, vc, ff) (irq & \ | |
847 | (CAL_CSI2_VC_IRQENABLE_ ##ff ##_IRQ_##vc ##_MASK)) | |
848 | ||
849 | #define isportirqset(irq, port) (irq & CAL_HL_IRQ_MASK(port)) | |
850 | ||
851 | static irqreturn_t cal_irq(int irq_cal, void *data) | |
852 | { | |
853 | struct cal_dev *dev = (struct cal_dev *)data; | |
854 | struct cal_ctx *ctx; | |
855 | struct cal_dmaqueue *dma_q; | |
856 | u32 irqst2, irqst3; | |
857 | ||
858 | /* Check which DMA just finished */ | |
859 | irqst2 = reg_read(dev, CAL_HL_IRQSTATUS(2)); | |
860 | if (irqst2) { | |
861 | /* Clear Interrupt status */ | |
862 | reg_write(dev, CAL_HL_IRQSTATUS(2), irqst2); | |
863 | ||
864 | /* Need to check both port */ | |
865 | if (isportirqset(irqst2, 1)) { | |
866 | ctx = dev->ctx[0]; | |
867 | ||
868 | if (ctx->cur_frm != ctx->next_frm) | |
869 | cal_process_buffer_complete(ctx); | |
870 | } | |
871 | ||
872 | if (isportirqset(irqst2, 2)) { | |
873 | ctx = dev->ctx[1]; | |
874 | ||
875 | if (ctx->cur_frm != ctx->next_frm) | |
876 | cal_process_buffer_complete(ctx); | |
877 | } | |
878 | } | |
879 | ||
880 | /* Check which DMA just started */ | |
881 | irqst3 = reg_read(dev, CAL_HL_IRQSTATUS(3)); | |
882 | if (irqst3) { | |
883 | /* Clear Interrupt status */ | |
884 | reg_write(dev, CAL_HL_IRQSTATUS(3), irqst3); | |
885 | ||
886 | /* Need to check both port */ | |
887 | if (isportirqset(irqst3, 1)) { | |
888 | ctx = dev->ctx[0]; | |
889 | dma_q = &ctx->vidq; | |
890 | ||
891 | spin_lock(&ctx->slock); | |
892 | if (!list_empty(&dma_q->active) && | |
893 | ctx->cur_frm == ctx->next_frm) | |
894 | cal_schedule_next_buffer(ctx); | |
895 | spin_unlock(&ctx->slock); | |
896 | } | |
897 | ||
898 | if (isportirqset(irqst3, 2)) { | |
899 | ctx = dev->ctx[1]; | |
900 | dma_q = &ctx->vidq; | |
901 | ||
902 | spin_lock(&ctx->slock); | |
903 | if (!list_empty(&dma_q->active) && | |
904 | ctx->cur_frm == ctx->next_frm) | |
905 | cal_schedule_next_buffer(ctx); | |
906 | spin_unlock(&ctx->slock); | |
907 | } | |
908 | } | |
909 | ||
910 | return IRQ_HANDLED; | |
911 | } | |
912 | ||
913 | /* | |
914 | * video ioctls | |
915 | */ | |
916 | static int cal_querycap(struct file *file, void *priv, | |
917 | struct v4l2_capability *cap) | |
918 | { | |
919 | struct cal_ctx *ctx = video_drvdata(file); | |
920 | ||
921 | strlcpy(cap->driver, CAL_MODULE_NAME, sizeof(cap->driver)); | |
922 | strlcpy(cap->card, CAL_MODULE_NAME, sizeof(cap->card)); | |
923 | ||
924 | snprintf(cap->bus_info, sizeof(cap->bus_info), | |
925 | "platform:%s", ctx->v4l2_dev.name); | |
926 | cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING | | |
927 | V4L2_CAP_READWRITE; | |
928 | cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS; | |
929 | return 0; | |
930 | } | |
931 | ||
932 | static int cal_enum_fmt_vid_cap(struct file *file, void *priv, | |
933 | struct v4l2_fmtdesc *f) | |
934 | { | |
935 | struct cal_ctx *ctx = video_drvdata(file); | |
936 | const struct cal_fmt *fmt = NULL; | |
937 | ||
938 | if (f->index >= ctx->num_active_fmt) | |
939 | return -EINVAL; | |
940 | ||
941 | fmt = ctx->active_fmt[f->index]; | |
942 | ||
943 | f->pixelformat = fmt->fourcc; | |
944 | f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
945 | return 0; | |
946 | } | |
947 | ||
948 | static int __subdev_get_format(struct cal_ctx *ctx, | |
949 | struct v4l2_mbus_framefmt *fmt) | |
950 | { | |
951 | struct v4l2_subdev_format sd_fmt; | |
952 | struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format; | |
953 | int ret; | |
954 | ||
343e89a7 BP |
955 | sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; |
956 | sd_fmt.pad = 0; | |
957 | ||
958 | ret = v4l2_subdev_call(ctx->sensor, pad, get_fmt, NULL, &sd_fmt); | |
959 | if (ret) | |
960 | return ret; | |
961 | ||
962 | *fmt = *mbus_fmt; | |
963 | ||
964 | ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__, | |
965 | fmt->width, fmt->height, fmt->code); | |
966 | ||
967 | return 0; | |
968 | } | |
969 | ||
970 | static int __subdev_set_format(struct cal_ctx *ctx, | |
971 | struct v4l2_mbus_framefmt *fmt) | |
972 | { | |
973 | struct v4l2_subdev_format sd_fmt; | |
974 | struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format; | |
975 | int ret; | |
976 | ||
343e89a7 BP |
977 | sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE; |
978 | sd_fmt.pad = 0; | |
979 | *mbus_fmt = *fmt; | |
980 | ||
981 | ret = v4l2_subdev_call(ctx->sensor, pad, set_fmt, NULL, &sd_fmt); | |
982 | if (ret) | |
983 | return ret; | |
984 | ||
985 | ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__, | |
986 | fmt->width, fmt->height, fmt->code); | |
987 | ||
988 | return 0; | |
989 | } | |
990 | ||
991 | static int cal_calc_format_size(struct cal_ctx *ctx, | |
992 | const struct cal_fmt *fmt, | |
993 | struct v4l2_format *f) | |
994 | { | |
995 | if (!fmt) { | |
996 | ctx_dbg(3, ctx, "No cal_fmt provided!\n"); | |
997 | return -EINVAL; | |
998 | } | |
999 | ||
1000 | v4l_bound_align_image(&f->fmt.pix.width, 48, MAX_WIDTH, 2, | |
1001 | &f->fmt.pix.height, 32, MAX_HEIGHT, 0, 0); | |
1002 | f->fmt.pix.bytesperline = bytes_per_line(f->fmt.pix.width, | |
1003 | fmt->depth >> 3); | |
1004 | f->fmt.pix.sizeimage = f->fmt.pix.height * | |
1005 | f->fmt.pix.bytesperline; | |
1006 | ||
1007 | ctx_dbg(3, ctx, "%s: fourcc: %s size: %dx%d bpl:%d img_size:%d\n", | |
1008 | __func__, fourcc_to_str(f->fmt.pix.pixelformat), | |
1009 | f->fmt.pix.width, f->fmt.pix.height, | |
1010 | f->fmt.pix.bytesperline, f->fmt.pix.sizeimage); | |
1011 | ||
1012 | return 0; | |
1013 | } | |
1014 | ||
1015 | static int cal_g_fmt_vid_cap(struct file *file, void *priv, | |
1016 | struct v4l2_format *f) | |
1017 | { | |
1018 | struct cal_ctx *ctx = video_drvdata(file); | |
1019 | ||
1020 | *f = ctx->v_fmt; | |
1021 | ||
1022 | return 0; | |
1023 | } | |
1024 | ||
1025 | static int cal_try_fmt_vid_cap(struct file *file, void *priv, | |
1026 | struct v4l2_format *f) | |
1027 | { | |
1028 | struct cal_ctx *ctx = video_drvdata(file); | |
1029 | const struct cal_fmt *fmt; | |
1030 | struct v4l2_subdev_frame_size_enum fse; | |
1031 | int ret, found; | |
1032 | ||
1033 | fmt = find_format_by_pix(ctx, f->fmt.pix.pixelformat); | |
1034 | if (!fmt) { | |
1035 | ctx_dbg(3, ctx, "Fourcc format (0x%08x) not found.\n", | |
1036 | f->fmt.pix.pixelformat); | |
1037 | ||
1038 | /* Just get the first one enumerated */ | |
1039 | fmt = ctx->active_fmt[0]; | |
1040 | f->fmt.pix.pixelformat = fmt->fourcc; | |
1041 | } | |
1042 | ||
1043 | f->fmt.pix.field = ctx->v_fmt.fmt.pix.field; | |
1044 | ||
1045 | /* check for/find a valid width/height */ | |
1046 | ret = 0; | |
1047 | found = false; | |
1048 | fse.pad = 0; | |
1049 | fse.code = fmt->code; | |
1050 | fse.which = V4L2_SUBDEV_FORMAT_ACTIVE; | |
1051 | for (fse.index = 0; ; fse.index++) { | |
1052 | ret = v4l2_subdev_call(ctx->sensor, pad, enum_frame_size, | |
1053 | NULL, &fse); | |
1054 | if (ret) | |
1055 | break; | |
1056 | ||
1057 | if ((f->fmt.pix.width == fse.max_width) && | |
1058 | (f->fmt.pix.height == fse.max_height)) { | |
1059 | found = true; | |
1060 | break; | |
1061 | } else if ((f->fmt.pix.width >= fse.min_width) && | |
1062 | (f->fmt.pix.width <= fse.max_width) && | |
1063 | (f->fmt.pix.height >= fse.min_height) && | |
1064 | (f->fmt.pix.height <= fse.max_height)) { | |
1065 | found = true; | |
1066 | break; | |
1067 | } | |
1068 | } | |
1069 | ||
1070 | if (!found) { | |
1071 | /* use existing values as default */ | |
1072 | f->fmt.pix.width = ctx->v_fmt.fmt.pix.width; | |
1073 | f->fmt.pix.height = ctx->v_fmt.fmt.pix.height; | |
1074 | } | |
1075 | ||
1076 | /* | |
1077 | * Use current colorspace for now, it will get | |
1078 | * updated properly during s_fmt | |
1079 | */ | |
1080 | f->fmt.pix.colorspace = ctx->v_fmt.fmt.pix.colorspace; | |
1081 | return cal_calc_format_size(ctx, fmt, f); | |
1082 | } | |
1083 | ||
1084 | static int cal_s_fmt_vid_cap(struct file *file, void *priv, | |
1085 | struct v4l2_format *f) | |
1086 | { | |
1087 | struct cal_ctx *ctx = video_drvdata(file); | |
1088 | struct vb2_queue *q = &ctx->vb_vidq; | |
1089 | const struct cal_fmt *fmt; | |
1090 | struct v4l2_mbus_framefmt mbus_fmt; | |
1091 | int ret; | |
1092 | ||
1093 | if (vb2_is_busy(q)) { | |
1094 | ctx_dbg(3, ctx, "%s device busy\n", __func__); | |
1095 | return -EBUSY; | |
1096 | } | |
1097 | ||
1098 | ret = cal_try_fmt_vid_cap(file, priv, f); | |
1099 | if (ret < 0) | |
1100 | return ret; | |
1101 | ||
1102 | fmt = find_format_by_pix(ctx, f->fmt.pix.pixelformat); | |
1103 | ||
1104 | v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, fmt->code); | |
1105 | ||
1106 | ret = __subdev_set_format(ctx, &mbus_fmt); | |
1107 | if (ret) | |
1108 | return ret; | |
1109 | ||
1110 | /* Just double check nothing has gone wrong */ | |
1111 | if (mbus_fmt.code != fmt->code) { | |
1112 | ctx_dbg(3, ctx, | |
1113 | "%s subdev changed format on us, this should not happen\n", | |
1114 | __func__); | |
1115 | return -EINVAL; | |
1116 | } | |
1117 | ||
1118 | v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt); | |
1119 | ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
1120 | ctx->v_fmt.fmt.pix.pixelformat = fmt->fourcc; | |
1121 | cal_calc_format_size(ctx, fmt, &ctx->v_fmt); | |
1122 | ctx->fmt = fmt; | |
1123 | ctx->m_fmt = mbus_fmt; | |
1124 | *f = ctx->v_fmt; | |
1125 | ||
1126 | return 0; | |
1127 | } | |
1128 | ||
1129 | static int cal_enum_framesizes(struct file *file, void *fh, | |
1130 | struct v4l2_frmsizeenum *fsize) | |
1131 | { | |
1132 | struct cal_ctx *ctx = video_drvdata(file); | |
1133 | const struct cal_fmt *fmt; | |
1134 | struct v4l2_subdev_frame_size_enum fse; | |
1135 | int ret; | |
1136 | ||
1137 | /* check for valid format */ | |
1138 | fmt = find_format_by_pix(ctx, fsize->pixel_format); | |
1139 | if (!fmt) { | |
1140 | ctx_dbg(3, ctx, "Invalid pixel code: %x\n", | |
1141 | fsize->pixel_format); | |
1142 | return -EINVAL; | |
1143 | } | |
1144 | ||
1145 | fse.index = fsize->index; | |
1146 | fse.pad = 0; | |
1147 | fse.code = fmt->code; | |
1148 | ||
1149 | ret = v4l2_subdev_call(ctx->sensor, pad, enum_frame_size, NULL, &fse); | |
1150 | if (ret) | |
2ddf22ee | 1151 | return ret; |
343e89a7 BP |
1152 | |
1153 | ctx_dbg(1, ctx, "%s: index: %d code: %x W:[%d,%d] H:[%d,%d]\n", | |
1154 | __func__, fse.index, fse.code, fse.min_width, fse.max_width, | |
1155 | fse.min_height, fse.max_height); | |
1156 | ||
1157 | fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE; | |
1158 | fsize->discrete.width = fse.max_width; | |
1159 | fsize->discrete.height = fse.max_height; | |
1160 | ||
1161 | return 0; | |
1162 | } | |
1163 | ||
1164 | static int cal_enum_input(struct file *file, void *priv, | |
1165 | struct v4l2_input *inp) | |
1166 | { | |
1167 | if (inp->index >= CAL_NUM_INPUT) | |
1168 | return -EINVAL; | |
1169 | ||
1170 | inp->type = V4L2_INPUT_TYPE_CAMERA; | |
1171 | sprintf(inp->name, "Camera %u", inp->index); | |
1172 | return 0; | |
1173 | } | |
1174 | ||
1175 | static int cal_g_input(struct file *file, void *priv, unsigned int *i) | |
1176 | { | |
1177 | struct cal_ctx *ctx = video_drvdata(file); | |
1178 | ||
1179 | *i = ctx->input; | |
1180 | return 0; | |
1181 | } | |
1182 | ||
1183 | static int cal_s_input(struct file *file, void *priv, unsigned int i) | |
1184 | { | |
1185 | struct cal_ctx *ctx = video_drvdata(file); | |
1186 | ||
1187 | if (i >= CAL_NUM_INPUT) | |
1188 | return -EINVAL; | |
1189 | ||
1190 | ctx->input = i; | |
1191 | return 0; | |
1192 | } | |
1193 | ||
1194 | /* timeperframe is arbitrary and continuous */ | |
1195 | static int cal_enum_frameintervals(struct file *file, void *priv, | |
1196 | struct v4l2_frmivalenum *fival) | |
1197 | { | |
1198 | struct cal_ctx *ctx = video_drvdata(file); | |
1199 | const struct cal_fmt *fmt; | |
7f67c587 BP |
1200 | struct v4l2_subdev_frame_interval_enum fie = { |
1201 | .index = fival->index, | |
1202 | .width = fival->width, | |
1203 | .height = fival->height, | |
1204 | .which = V4L2_SUBDEV_FORMAT_ACTIVE, | |
1205 | }; | |
343e89a7 BP |
1206 | int ret; |
1207 | ||
343e89a7 BP |
1208 | fmt = find_format_by_pix(ctx, fival->pixel_format); |
1209 | if (!fmt) | |
1210 | return -EINVAL; | |
1211 | ||
7f67c587 BP |
1212 | fie.code = fmt->code; |
1213 | ret = v4l2_subdev_call(ctx->sensor, pad, enum_frame_interval, | |
1214 | NULL, &fie); | |
1215 | if (ret) | |
1216 | return ret; | |
343e89a7 | 1217 | fival->type = V4L2_FRMIVAL_TYPE_DISCRETE; |
7f67c587 | 1218 | fival->discrete = fie.interval; |
343e89a7 BP |
1219 | |
1220 | return 0; | |
1221 | } | |
1222 | ||
1223 | /* | |
1224 | * Videobuf operations | |
1225 | */ | |
1226 | static int cal_queue_setup(struct vb2_queue *vq, | |
1227 | unsigned int *nbuffers, unsigned int *nplanes, | |
36c0f8b3 | 1228 | unsigned int sizes[], struct device *alloc_devs[]) |
343e89a7 BP |
1229 | { |
1230 | struct cal_ctx *ctx = vb2_get_drv_priv(vq); | |
1231 | unsigned size = ctx->v_fmt.fmt.pix.sizeimage; | |
1232 | ||
1233 | if (vq->num_buffers + *nbuffers < 3) | |
1234 | *nbuffers = 3 - vq->num_buffers; | |
343e89a7 BP |
1235 | |
1236 | if (*nplanes) { | |
1237 | if (sizes[0] < size) | |
1238 | return -EINVAL; | |
1239 | size = sizes[0]; | |
1240 | } | |
1241 | ||
1242 | *nplanes = 1; | |
1243 | sizes[0] = size; | |
1244 | ||
1245 | ctx_dbg(3, ctx, "nbuffers=%d, size=%d\n", *nbuffers, sizes[0]); | |
1246 | ||
1247 | return 0; | |
1248 | } | |
1249 | ||
1250 | static int cal_buffer_prepare(struct vb2_buffer *vb) | |
1251 | { | |
1252 | struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); | |
1253 | struct cal_buffer *buf = container_of(vb, struct cal_buffer, | |
1254 | vb.vb2_buf); | |
1255 | unsigned long size; | |
1256 | ||
1257 | if (WARN_ON(!ctx->fmt)) | |
1258 | return -EINVAL; | |
1259 | ||
1260 | size = ctx->v_fmt.fmt.pix.sizeimage; | |
1261 | if (vb2_plane_size(vb, 0) < size) { | |
1262 | ctx_err(ctx, | |
1263 | "data will not fit into plane (%lu < %lu)\n", | |
1264 | vb2_plane_size(vb, 0), size); | |
1265 | return -EINVAL; | |
1266 | } | |
1267 | ||
1268 | vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size); | |
1269 | return 0; | |
1270 | } | |
1271 | ||
1272 | static void cal_buffer_queue(struct vb2_buffer *vb) | |
1273 | { | |
1274 | struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue); | |
1275 | struct cal_buffer *buf = container_of(vb, struct cal_buffer, | |
1276 | vb.vb2_buf); | |
1277 | struct cal_dmaqueue *vidq = &ctx->vidq; | |
1278 | unsigned long flags = 0; | |
1279 | ||
1280 | /* recheck locking */ | |
1281 | spin_lock_irqsave(&ctx->slock, flags); | |
1282 | list_add_tail(&buf->list, &vidq->active); | |
1283 | spin_unlock_irqrestore(&ctx->slock, flags); | |
1284 | } | |
1285 | ||
1286 | static int cal_start_streaming(struct vb2_queue *vq, unsigned int count) | |
1287 | { | |
1288 | struct cal_ctx *ctx = vb2_get_drv_priv(vq); | |
1289 | struct cal_dmaqueue *dma_q = &ctx->vidq; | |
1290 | struct cal_buffer *buf, *tmp; | |
1291 | unsigned long addr = 0; | |
1292 | unsigned long flags; | |
1293 | int ret; | |
1294 | ||
1295 | spin_lock_irqsave(&ctx->slock, flags); | |
1296 | if (list_empty(&dma_q->active)) { | |
1297 | spin_unlock_irqrestore(&ctx->slock, flags); | |
1298 | ctx_dbg(3, ctx, "buffer queue is empty\n"); | |
1299 | return -EIO; | |
1300 | } | |
1301 | ||
1302 | buf = list_entry(dma_q->active.next, struct cal_buffer, list); | |
1303 | ctx->cur_frm = buf; | |
1304 | ctx->next_frm = buf; | |
1305 | list_del(&buf->list); | |
1306 | spin_unlock_irqrestore(&ctx->slock, flags); | |
1307 | ||
1308 | addr = vb2_dma_contig_plane_dma_addr(&ctx->cur_frm->vb.vb2_buf, 0); | |
1309 | ctx->sequence = 0; | |
1310 | ||
1311 | ret = cal_get_external_info(ctx); | |
1312 | if (ret < 0) | |
1313 | goto err; | |
1314 | ||
1315 | cal_runtime_get(ctx->dev); | |
1316 | ||
1317 | enable_irqs(ctx); | |
1318 | camerarx_phy_enable(ctx); | |
1319 | csi2_init(ctx); | |
1320 | csi2_phy_config(ctx); | |
1321 | csi2_lane_config(ctx); | |
1322 | csi2_ctx_config(ctx); | |
1323 | pix_proc_config(ctx); | |
1324 | cal_wr_dma_config(ctx, ctx->v_fmt.fmt.pix.bytesperline); | |
1325 | cal_wr_dma_addr(ctx, addr); | |
1326 | csi2_ppi_enable(ctx); | |
1327 | ||
2ddf22ee BP |
1328 | ret = v4l2_subdev_call(ctx->sensor, video, s_stream, 1); |
1329 | if (ret) { | |
1330 | ctx_err(ctx, "stream on failed in subdev\n"); | |
1331 | cal_runtime_put(ctx->dev); | |
1332 | goto err; | |
343e89a7 BP |
1333 | } |
1334 | ||
1335 | if (debug >= 4) | |
1336 | cal_quickdump_regs(ctx->dev); | |
1337 | ||
1338 | return 0; | |
1339 | ||
1340 | err: | |
1341 | list_for_each_entry_safe(buf, tmp, &dma_q->active, list) { | |
1342 | list_del(&buf->list); | |
1343 | vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED); | |
1344 | } | |
1345 | return ret; | |
1346 | } | |
1347 | ||
1348 | static void cal_stop_streaming(struct vb2_queue *vq) | |
1349 | { | |
1350 | struct cal_ctx *ctx = vb2_get_drv_priv(vq); | |
1351 | struct cal_dmaqueue *dma_q = &ctx->vidq; | |
1352 | struct cal_buffer *buf, *tmp; | |
1353 | unsigned long flags; | |
1354 | ||
2ddf22ee BP |
1355 | if (v4l2_subdev_call(ctx->sensor, video, s_stream, 0)) |
1356 | ctx_err(ctx, "stream off failed in subdev\n"); | |
343e89a7 BP |
1357 | |
1358 | csi2_ppi_disable(ctx); | |
1359 | disable_irqs(ctx); | |
1360 | ||
1361 | /* Release all active buffers */ | |
1362 | spin_lock_irqsave(&ctx->slock, flags); | |
1363 | list_for_each_entry_safe(buf, tmp, &dma_q->active, list) { | |
1364 | list_del(&buf->list); | |
1365 | vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); | |
1366 | } | |
1367 | ||
1368 | if (ctx->cur_frm == ctx->next_frm) { | |
1369 | vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR); | |
1370 | } else { | |
1371 | vb2_buffer_done(&ctx->cur_frm->vb.vb2_buf, VB2_BUF_STATE_ERROR); | |
1372 | vb2_buffer_done(&ctx->next_frm->vb.vb2_buf, | |
1373 | VB2_BUF_STATE_ERROR); | |
1374 | } | |
1375 | ctx->cur_frm = NULL; | |
1376 | ctx->next_frm = NULL; | |
1377 | spin_unlock_irqrestore(&ctx->slock, flags); | |
1378 | ||
1379 | cal_runtime_put(ctx->dev); | |
1380 | } | |
1381 | ||
1382 | static struct vb2_ops cal_video_qops = { | |
1383 | .queue_setup = cal_queue_setup, | |
1384 | .buf_prepare = cal_buffer_prepare, | |
1385 | .buf_queue = cal_buffer_queue, | |
1386 | .start_streaming = cal_start_streaming, | |
1387 | .stop_streaming = cal_stop_streaming, | |
1388 | .wait_prepare = vb2_ops_wait_prepare, | |
1389 | .wait_finish = vb2_ops_wait_finish, | |
1390 | }; | |
1391 | ||
1392 | static const struct v4l2_file_operations cal_fops = { | |
1393 | .owner = THIS_MODULE, | |
1394 | .open = v4l2_fh_open, | |
1395 | .release = vb2_fop_release, | |
1396 | .read = vb2_fop_read, | |
1397 | .poll = vb2_fop_poll, | |
1398 | .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */ | |
1399 | .mmap = vb2_fop_mmap, | |
1400 | }; | |
1401 | ||
1402 | static const struct v4l2_ioctl_ops cal_ioctl_ops = { | |
1403 | .vidioc_querycap = cal_querycap, | |
1404 | .vidioc_enum_fmt_vid_cap = cal_enum_fmt_vid_cap, | |
1405 | .vidioc_g_fmt_vid_cap = cal_g_fmt_vid_cap, | |
1406 | .vidioc_try_fmt_vid_cap = cal_try_fmt_vid_cap, | |
1407 | .vidioc_s_fmt_vid_cap = cal_s_fmt_vid_cap, | |
1408 | .vidioc_enum_framesizes = cal_enum_framesizes, | |
1409 | .vidioc_reqbufs = vb2_ioctl_reqbufs, | |
1410 | .vidioc_create_bufs = vb2_ioctl_create_bufs, | |
1411 | .vidioc_prepare_buf = vb2_ioctl_prepare_buf, | |
1412 | .vidioc_querybuf = vb2_ioctl_querybuf, | |
1413 | .vidioc_qbuf = vb2_ioctl_qbuf, | |
1414 | .vidioc_dqbuf = vb2_ioctl_dqbuf, | |
1415 | .vidioc_enum_input = cal_enum_input, | |
1416 | .vidioc_g_input = cal_g_input, | |
1417 | .vidioc_s_input = cal_s_input, | |
1418 | .vidioc_enum_frameintervals = cal_enum_frameintervals, | |
1419 | .vidioc_streamon = vb2_ioctl_streamon, | |
1420 | .vidioc_streamoff = vb2_ioctl_streamoff, | |
1421 | .vidioc_log_status = v4l2_ctrl_log_status, | |
1422 | .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, | |
1423 | .vidioc_unsubscribe_event = v4l2_event_unsubscribe, | |
1424 | }; | |
1425 | ||
1426 | static struct video_device cal_videodev = { | |
1427 | .name = CAL_MODULE_NAME, | |
1428 | .fops = &cal_fops, | |
1429 | .ioctl_ops = &cal_ioctl_ops, | |
1430 | .minor = -1, | |
1431 | .release = video_device_release_empty, | |
1432 | }; | |
1433 | ||
1434 | /* ----------------------------------------------------------------- | |
1435 | * Initialization and module stuff | |
1436 | * ------------------------------------------------------------------ | |
1437 | */ | |
1438 | static int cal_complete_ctx(struct cal_ctx *ctx); | |
1439 | ||
1440 | static int cal_async_bound(struct v4l2_async_notifier *notifier, | |
1441 | struct v4l2_subdev *subdev, | |
1442 | struct v4l2_async_subdev *asd) | |
1443 | { | |
1444 | struct cal_ctx *ctx = notifier_to_ctx(notifier); | |
1445 | struct v4l2_subdev_mbus_code_enum mbus_code; | |
1446 | int ret = 0; | |
1447 | int i, j, k; | |
1448 | ||
1449 | if (ctx->sensor) { | |
1450 | ctx_info(ctx, "Rejecting subdev %s (Already set!!)", | |
1451 | subdev->name); | |
1452 | return 0; | |
1453 | } | |
1454 | ||
1455 | ctx->sensor = subdev; | |
1456 | ctx_dbg(1, ctx, "Using sensor %s for capture\n", subdev->name); | |
1457 | ||
1458 | /* Enumerate sub device formats and enable all matching local formats */ | |
1459 | ctx->num_active_fmt = 0; | |
1460 | for (j = 0, i = 0; ret != -EINVAL; ++j) { | |
1461 | struct cal_fmt *fmt; | |
1462 | ||
1463 | memset(&mbus_code, 0, sizeof(mbus_code)); | |
1464 | mbus_code.index = j; | |
1465 | ret = v4l2_subdev_call(subdev, pad, enum_mbus_code, | |
1466 | NULL, &mbus_code); | |
1467 | if (ret) | |
1468 | continue; | |
1469 | ||
1470 | ctx_dbg(2, ctx, | |
1471 | "subdev %s: code: %04x idx: %d\n", | |
1472 | subdev->name, mbus_code.code, j); | |
1473 | ||
1474 | for (k = 0; k < ARRAY_SIZE(cal_formats); k++) { | |
1475 | fmt = &cal_formats[k]; | |
1476 | ||
1477 | if (mbus_code.code == fmt->code) { | |
1478 | ctx->active_fmt[i] = fmt; | |
1479 | ctx_dbg(2, ctx, | |
1480 | "matched fourcc: %s: code: %04x idx: %d\n", | |
1481 | fourcc_to_str(fmt->fourcc), | |
1482 | fmt->code, i); | |
1483 | ctx->num_active_fmt = ++i; | |
1484 | } | |
1485 | } | |
1486 | } | |
1487 | ||
1488 | if (i == 0) { | |
1489 | ctx_err(ctx, "No suitable format reported by subdev %s\n", | |
1490 | subdev->name); | |
1491 | return -EINVAL; | |
1492 | } | |
1493 | ||
1494 | cal_complete_ctx(ctx); | |
1495 | ||
1496 | return 0; | |
1497 | } | |
1498 | ||
1499 | static int cal_async_complete(struct v4l2_async_notifier *notifier) | |
1500 | { | |
1501 | struct cal_ctx *ctx = notifier_to_ctx(notifier); | |
1502 | const struct cal_fmt *fmt; | |
1503 | struct v4l2_mbus_framefmt mbus_fmt; | |
1504 | int ret; | |
1505 | ||
1506 | ret = __subdev_get_format(ctx, &mbus_fmt); | |
1507 | if (ret) | |
1508 | return ret; | |
1509 | ||
1510 | fmt = find_format_by_code(ctx, mbus_fmt.code); | |
1511 | if (!fmt) { | |
1512 | ctx_dbg(3, ctx, "mbus code format (0x%08x) not found.\n", | |
1513 | mbus_fmt.code); | |
1514 | return -EINVAL; | |
1515 | } | |
1516 | ||
1517 | /* Save current subdev format */ | |
1518 | v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt); | |
1519 | ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
1520 | ctx->v_fmt.fmt.pix.pixelformat = fmt->fourcc; | |
1521 | cal_calc_format_size(ctx, fmt, &ctx->v_fmt); | |
1522 | ctx->fmt = fmt; | |
1523 | ctx->m_fmt = mbus_fmt; | |
1524 | ||
1525 | return 0; | |
1526 | } | |
1527 | ||
1528 | static int cal_complete_ctx(struct cal_ctx *ctx) | |
1529 | { | |
1530 | struct video_device *vfd; | |
1531 | struct vb2_queue *q; | |
1532 | int ret; | |
1533 | ||
1534 | ctx->timeperframe = tpf_default; | |
1535 | ctx->external_rate = 192000000; | |
1536 | ||
1537 | /* initialize locks */ | |
1538 | spin_lock_init(&ctx->slock); | |
1539 | mutex_init(&ctx->mutex); | |
1540 | ||
1541 | /* initialize queue */ | |
1542 | q = &ctx->vb_vidq; | |
1543 | q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; | |
1544 | q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_READ; | |
1545 | q->drv_priv = ctx; | |
1546 | q->buf_struct_size = sizeof(struct cal_buffer); | |
1547 | q->ops = &cal_video_qops; | |
1548 | q->mem_ops = &vb2_dma_contig_memops; | |
1549 | q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; | |
1550 | q->lock = &ctx->mutex; | |
1551 | q->min_buffers_needed = 3; | |
dce57314 | 1552 | q->dev = ctx->v4l2_dev.dev; |
343e89a7 BP |
1553 | |
1554 | ret = vb2_queue_init(q); | |
1555 | if (ret) | |
1556 | return ret; | |
1557 | ||
1558 | /* init video dma queues */ | |
1559 | INIT_LIST_HEAD(&ctx->vidq.active); | |
1560 | ||
1561 | vfd = &ctx->vdev; | |
1562 | *vfd = cal_videodev; | |
1563 | vfd->v4l2_dev = &ctx->v4l2_dev; | |
1564 | vfd->queue = q; | |
1565 | ||
1566 | /* | |
1567 | * Provide a mutex to v4l2 core. It will be used to protect | |
1568 | * all fops and v4l2 ioctls. | |
1569 | */ | |
1570 | vfd->lock = &ctx->mutex; | |
1571 | video_set_drvdata(vfd, ctx); | |
1572 | ||
1573 | ret = video_register_device(vfd, VFL_TYPE_GRABBER, video_nr); | |
1574 | if (ret < 0) | |
1575 | return ret; | |
1576 | ||
1577 | v4l2_info(&ctx->v4l2_dev, "V4L2 device registered as %s\n", | |
1578 | video_device_node_name(vfd)); | |
1579 | ||
343e89a7 | 1580 | return 0; |
343e89a7 BP |
1581 | } |
1582 | ||
1583 | static struct device_node * | |
1584 | of_get_next_port(const struct device_node *parent, | |
1585 | struct device_node *prev) | |
1586 | { | |
1587 | struct device_node *port = NULL; | |
1588 | ||
1589 | if (!parent) | |
1590 | return NULL; | |
1591 | ||
1592 | if (!prev) { | |
1593 | struct device_node *ports; | |
1594 | /* | |
1595 | * It's the first call, we have to find a port subnode | |
1596 | * within this node or within an optional 'ports' node. | |
1597 | */ | |
1598 | ports = of_get_child_by_name(parent, "ports"); | |
1599 | if (ports) | |
1600 | parent = ports; | |
1601 | ||
1602 | port = of_get_child_by_name(parent, "port"); | |
1603 | ||
1604 | /* release the 'ports' node */ | |
1605 | of_node_put(ports); | |
1606 | } else { | |
1607 | struct device_node *ports; | |
1608 | ||
1609 | ports = of_get_parent(prev); | |
1610 | if (!ports) | |
1611 | return NULL; | |
1612 | ||
1613 | do { | |
1614 | port = of_get_next_child(ports, prev); | |
1615 | if (!port) { | |
1616 | of_node_put(ports); | |
1617 | return NULL; | |
1618 | } | |
1619 | prev = port; | |
1620 | } while (of_node_cmp(port->name, "port") != 0); | |
1621 | } | |
1622 | ||
1623 | return port; | |
1624 | } | |
1625 | ||
1626 | static struct device_node * | |
1627 | of_get_next_endpoint(const struct device_node *parent, | |
1628 | struct device_node *prev) | |
1629 | { | |
1630 | struct device_node *ep = NULL; | |
1631 | ||
1632 | if (!parent) | |
1633 | return NULL; | |
1634 | ||
1635 | do { | |
1636 | ep = of_get_next_child(parent, prev); | |
1637 | if (!ep) | |
1638 | return NULL; | |
1639 | prev = ep; | |
1640 | } while (of_node_cmp(ep->name, "endpoint") != 0); | |
1641 | ||
1642 | return ep; | |
1643 | } | |
1644 | ||
1645 | static int of_cal_create_instance(struct cal_ctx *ctx, int inst) | |
1646 | { | |
1647 | struct platform_device *pdev = ctx->dev->pdev; | |
1648 | struct device_node *ep_node, *port, *remote_ep, | |
1649 | *sensor_node, *parent; | |
1650 | struct v4l2_of_endpoint *endpoint; | |
1651 | struct v4l2_async_subdev *asd; | |
1652 | u32 regval = 0; | |
1653 | int ret, index, found_port = 0, lane; | |
1654 | ||
1655 | parent = pdev->dev.of_node; | |
1656 | ||
1657 | asd = &ctx->asd; | |
1658 | endpoint = &ctx->endpoint; | |
1659 | ||
1660 | ep_node = NULL; | |
1661 | port = NULL; | |
1662 | remote_ep = NULL; | |
1663 | sensor_node = NULL; | |
1664 | ret = -EINVAL; | |
1665 | ||
1666 | ctx_dbg(3, ctx, "Scanning Port node for csi2 port: %d\n", inst); | |
1667 | for (index = 0; index < CAL_NUM_CSI2_PORTS; index++) { | |
1668 | port = of_get_next_port(parent, port); | |
1669 | if (!port) { | |
1670 | ctx_dbg(1, ctx, "No port node found for csi2 port:%d\n", | |
1671 | index); | |
1672 | goto cleanup_exit; | |
1673 | } | |
1674 | ||
1675 | /* Match the slice number with <REG> */ | |
1676 | of_property_read_u32(port, "reg", ®val); | |
1677 | ctx_dbg(3, ctx, "port:%d inst:%d <reg>:%d\n", | |
1678 | index, inst, regval); | |
1679 | if ((regval == inst) && (index == inst)) { | |
1680 | found_port = 1; | |
1681 | break; | |
1682 | } | |
1683 | } | |
1684 | ||
1685 | if (!found_port) { | |
1686 | ctx_dbg(1, ctx, "No port node matches csi2 port:%d\n", | |
1687 | inst); | |
1688 | goto cleanup_exit; | |
1689 | } | |
1690 | ||
1691 | ctx_dbg(3, ctx, "Scanning sub-device for csi2 port: %d\n", | |
1692 | inst); | |
1693 | ||
1694 | ep_node = of_get_next_endpoint(port, ep_node); | |
1695 | if (!ep_node) { | |
1696 | ctx_dbg(3, ctx, "can't get next endpoint\n"); | |
1697 | goto cleanup_exit; | |
1698 | } | |
1699 | ||
1700 | sensor_node = of_graph_get_remote_port_parent(ep_node); | |
1701 | if (!sensor_node) { | |
1702 | ctx_dbg(3, ctx, "can't get remote parent\n"); | |
1703 | goto cleanup_exit; | |
1704 | } | |
1705 | asd->match_type = V4L2_ASYNC_MATCH_OF; | |
1706 | asd->match.of.node = sensor_node; | |
1707 | ||
1708 | remote_ep = of_parse_phandle(ep_node, "remote-endpoint", 0); | |
1709 | if (!remote_ep) { | |
1710 | ctx_dbg(3, ctx, "can't get remote-endpoint\n"); | |
1711 | goto cleanup_exit; | |
1712 | } | |
1713 | v4l2_of_parse_endpoint(remote_ep, endpoint); | |
1714 | ||
1715 | if (endpoint->bus_type != V4L2_MBUS_CSI2) { | |
1716 | ctx_err(ctx, "Port:%d sub-device %s is not a CSI2 device\n", | |
1717 | inst, sensor_node->name); | |
1718 | goto cleanup_exit; | |
1719 | } | |
1720 | ||
1721 | /* Store Virtual Channel number */ | |
1722 | ctx->virtual_channel = endpoint->base.id; | |
1723 | ||
1724 | ctx_dbg(3, ctx, "Port:%d v4l2-endpoint: CSI2\n", inst); | |
1725 | ctx_dbg(3, ctx, "Virtual Channel=%d\n", ctx->virtual_channel); | |
1726 | ctx_dbg(3, ctx, "flags=0x%08x\n", endpoint->bus.mipi_csi2.flags); | |
1727 | ctx_dbg(3, ctx, "clock_lane=%d\n", endpoint->bus.mipi_csi2.clock_lane); | |
1728 | ctx_dbg(3, ctx, "num_data_lanes=%d\n", | |
1729 | endpoint->bus.mipi_csi2.num_data_lanes); | |
1730 | ctx_dbg(3, ctx, "data_lanes= <\n"); | |
1731 | for (lane = 0; lane < endpoint->bus.mipi_csi2.num_data_lanes; lane++) | |
1732 | ctx_dbg(3, ctx, "\t%d\n", | |
1733 | endpoint->bus.mipi_csi2.data_lanes[lane]); | |
1734 | ctx_dbg(3, ctx, "\t>\n"); | |
1735 | ||
1736 | ctx_dbg(1, ctx, "Port: %d found sub-device %s\n", | |
1737 | inst, sensor_node->name); | |
1738 | ||
1739 | ctx->asd_list[0] = asd; | |
1740 | ctx->notifier.subdevs = ctx->asd_list; | |
1741 | ctx->notifier.num_subdevs = 1; | |
1742 | ctx->notifier.bound = cal_async_bound; | |
1743 | ctx->notifier.complete = cal_async_complete; | |
1744 | ret = v4l2_async_notifier_register(&ctx->v4l2_dev, | |
1745 | &ctx->notifier); | |
1746 | if (ret) { | |
1747 | ctx_err(ctx, "Error registering async notifier\n"); | |
1748 | ret = -EINVAL; | |
1749 | } | |
1750 | ||
1751 | cleanup_exit: | |
1752 | if (!remote_ep) | |
1753 | of_node_put(remote_ep); | |
1754 | if (!sensor_node) | |
1755 | of_node_put(sensor_node); | |
1756 | if (!ep_node) | |
1757 | of_node_put(ep_node); | |
1758 | if (!port) | |
1759 | of_node_put(port); | |
1760 | ||
1761 | return ret; | |
1762 | } | |
1763 | ||
1764 | static struct cal_ctx *cal_create_instance(struct cal_dev *dev, int inst) | |
1765 | { | |
1766 | struct cal_ctx *ctx; | |
1767 | struct v4l2_ctrl_handler *hdl; | |
1768 | int ret; | |
1769 | ||
1770 | ctx = devm_kzalloc(&dev->pdev->dev, sizeof(*ctx), GFP_KERNEL); | |
1771 | if (!ctx) | |
96621112 | 1772 | return NULL; |
343e89a7 BP |
1773 | |
1774 | /* save the cal_dev * for future ref */ | |
1775 | ctx->dev = dev; | |
1776 | ||
1777 | snprintf(ctx->v4l2_dev.name, sizeof(ctx->v4l2_dev.name), | |
1778 | "%s-%03d", CAL_MODULE_NAME, inst); | |
1779 | ret = v4l2_device_register(&dev->pdev->dev, &ctx->v4l2_dev); | |
1780 | if (ret) | |
1781 | goto err_exit; | |
1782 | ||
1783 | hdl = &ctx->ctrl_handler; | |
1784 | ret = v4l2_ctrl_handler_init(hdl, 11); | |
1785 | if (ret) { | |
1786 | ctx_err(ctx, "Failed to init ctrl handler\n"); | |
1787 | goto unreg_dev; | |
1788 | } | |
1789 | ctx->v4l2_dev.ctrl_handler = hdl; | |
1790 | ||
1791 | /* Make sure Camera Core H/W register area is available */ | |
1792 | ctx->cc = dev->cc[inst]; | |
1793 | ||
1794 | /* Store the instance id */ | |
1795 | ctx->csi2_port = inst + 1; | |
1796 | ||
1797 | ret = of_cal_create_instance(ctx, inst); | |
1798 | if (ret) { | |
1799 | ret = -EINVAL; | |
1800 | goto free_hdl; | |
1801 | } | |
1802 | return ctx; | |
1803 | ||
1804 | free_hdl: | |
1805 | v4l2_ctrl_handler_free(hdl); | |
1806 | unreg_dev: | |
1807 | v4l2_device_unregister(&ctx->v4l2_dev); | |
1808 | err_exit: | |
96621112 | 1809 | return NULL; |
343e89a7 BP |
1810 | } |
1811 | ||
1812 | static int cal_probe(struct platform_device *pdev) | |
1813 | { | |
1814 | struct cal_dev *dev; | |
1815 | int ret; | |
1816 | int irq; | |
1817 | ||
1818 | dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL); | |
1819 | if (!dev) | |
1820 | return -ENOMEM; | |
1821 | ||
1822 | /* set pseudo v4l2 device name so we can use v4l2_printk */ | |
1823 | strlcpy(dev->v4l2_dev.name, CAL_MODULE_NAME, | |
1824 | sizeof(dev->v4l2_dev.name)); | |
1825 | ||
1826 | /* save pdev pointer */ | |
1827 | dev->pdev = pdev; | |
1828 | ||
1829 | dev->res = platform_get_resource_byname(pdev, IORESOURCE_MEM, | |
1830 | "cal_top"); | |
1831 | dev->base = devm_ioremap_resource(&pdev->dev, dev->res); | |
1832 | if (IS_ERR(dev->base)) | |
1833 | return PTR_ERR(dev->base); | |
1834 | ||
1835 | cal_dbg(1, dev, "ioresource %s at %pa - %pa\n", | |
1836 | dev->res->name, &dev->res->start, &dev->res->end); | |
1837 | ||
1838 | irq = platform_get_irq(pdev, 0); | |
1839 | cal_dbg(1, dev, "got irq# %d\n", irq); | |
1840 | ret = devm_request_irq(&pdev->dev, irq, cal_irq, 0, CAL_MODULE_NAME, | |
1841 | dev); | |
1842 | if (ret) | |
1843 | return ret; | |
1844 | ||
1845 | platform_set_drvdata(pdev, dev); | |
1846 | ||
1847 | dev->cm = cm_create(dev); | |
1848 | if (IS_ERR(dev->cm)) | |
1849 | return PTR_ERR(dev->cm); | |
1850 | ||
1851 | dev->cc[0] = cc_create(dev, 0); | |
1852 | if (IS_ERR(dev->cc[0])) | |
1853 | return PTR_ERR(dev->cc[0]); | |
1854 | ||
1855 | dev->cc[1] = cc_create(dev, 1); | |
1856 | if (IS_ERR(dev->cc[1])) | |
1857 | return PTR_ERR(dev->cc[1]); | |
1858 | ||
1859 | dev->ctx[0] = NULL; | |
1860 | dev->ctx[1] = NULL; | |
1861 | ||
1862 | dev->ctx[0] = cal_create_instance(dev, 0); | |
1863 | dev->ctx[1] = cal_create_instance(dev, 1); | |
1864 | if (!dev->ctx[0] && !dev->ctx[1]) { | |
1865 | cal_err(dev, "Neither port is configured, no point in staying up\n"); | |
1866 | return -ENODEV; | |
1867 | } | |
1868 | ||
1869 | pm_runtime_enable(&pdev->dev); | |
1870 | ||
1871 | ret = cal_runtime_get(dev); | |
1872 | if (ret) | |
1873 | goto runtime_disable; | |
1874 | ||
1875 | /* Just check we can actually access the module */ | |
1876 | cal_get_hwinfo(dev); | |
1877 | ||
1878 | cal_runtime_put(dev); | |
1879 | ||
1880 | return 0; | |
1881 | ||
1882 | runtime_disable: | |
1883 | pm_runtime_disable(&pdev->dev); | |
1884 | return ret; | |
1885 | } | |
1886 | ||
1887 | static int cal_remove(struct platform_device *pdev) | |
1888 | { | |
1889 | struct cal_dev *dev = | |
1890 | (struct cal_dev *)platform_get_drvdata(pdev); | |
1891 | struct cal_ctx *ctx; | |
1892 | int i; | |
1893 | ||
1894 | cal_dbg(1, dev, "Removing %s\n", CAL_MODULE_NAME); | |
1895 | ||
1896 | cal_runtime_get(dev); | |
1897 | ||
1898 | for (i = 0; i < CAL_NUM_CONTEXT; i++) { | |
1899 | ctx = dev->ctx[i]; | |
1900 | if (ctx) { | |
1901 | ctx_dbg(1, ctx, "unregistering %s\n", | |
1902 | video_device_node_name(&ctx->vdev)); | |
1903 | camerarx_phy_disable(ctx); | |
1904 | v4l2_async_notifier_unregister(&ctx->notifier); | |
343e89a7 BP |
1905 | v4l2_ctrl_handler_free(&ctx->ctrl_handler); |
1906 | v4l2_device_unregister(&ctx->v4l2_dev); | |
1907 | video_unregister_device(&ctx->vdev); | |
1908 | } | |
1909 | } | |
1910 | ||
1911 | cal_runtime_put(dev); | |
1912 | pm_runtime_disable(&pdev->dev); | |
1913 | ||
1914 | return 0; | |
1915 | } | |
1916 | ||
1917 | #if defined(CONFIG_OF) | |
1918 | static const struct of_device_id cal_of_match[] = { | |
1919 | { .compatible = "ti,dra72-cal", }, | |
1920 | {}, | |
1921 | }; | |
1922 | MODULE_DEVICE_TABLE(of, cal_of_match); | |
1923 | #endif | |
1924 | ||
1925 | static struct platform_driver cal_pdrv = { | |
1926 | .probe = cal_probe, | |
1927 | .remove = cal_remove, | |
1928 | .driver = { | |
1929 | .name = CAL_MODULE_NAME, | |
1930 | .of_match_table = of_match_ptr(cal_of_match), | |
1931 | }, | |
1932 | }; | |
1933 | ||
1934 | module_platform_driver(cal_pdrv); |