1 // SPDX-License-Identifier: (GPL-2.0-only OR MIT)
2 /*
3 * Copyright (C) 2024 Amlogic, Inc. All rights reserved
4 */
5
6 #include <linux/media/amlogic/c3-isp-config.h>
7 #include <linux/pm_runtime.h>
8
9 #include <media/v4l2-event.h>
10
11 #include "c3-isp-common.h"
12 #include "c3-isp-regs.h"
13
14 #define C3_ISP_CORE_SUBDEV_NAME "c3-isp-core"
15
16 #define C3_ISP_PHASE_OFFSET_0 0
17 #define C3_ISP_PHASE_OFFSET_1 1
18 #define C3_ISP_PHASE_OFFSET_NONE 0xff
19
20 #define C3_ISP_CORE_DEF_SINK_PAD_FMT MEDIA_BUS_FMT_SRGGB10_1X10
21 #define C3_ISP_CORE_DEF_SRC_PAD_FMT MEDIA_BUS_FMT_YUV10_1X30
22
23 /*
24 * struct c3_isp_core_format_info - ISP core format information
25 *
26 * @mbus_code: the mbus code
27 * @pads: bitmask detailing valid pads for this mbus_code
28 * @xofst: horizontal phase offset of hardware
29 * @yofst: vertical phase offset of hardware
30 * @is_raw: the raw format flag of mbus code
31 */
32 struct c3_isp_core_format_info {
33 u32 mbus_code;
34 u32 pads;
35 u8 xofst;
36 u8 yofst;
37 bool is_raw;
38 };
39
40 static const struct c3_isp_core_format_info c3_isp_core_fmts[] = {
41 /* RAW formats */
42 {
43 .mbus_code = MEDIA_BUS_FMT_SBGGR10_1X10,
44 .pads = BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
45 .xofst = C3_ISP_PHASE_OFFSET_0,
46 .yofst = C3_ISP_PHASE_OFFSET_1,
47 .is_raw = true,
48 }, {
49 .mbus_code = MEDIA_BUS_FMT_SGBRG10_1X10,
50 .pads = BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
51 .xofst = C3_ISP_PHASE_OFFSET_1,
52 .yofst = C3_ISP_PHASE_OFFSET_1,
53 .is_raw = true,
54 }, {
55 .mbus_code = MEDIA_BUS_FMT_SGRBG10_1X10,
56 .pads = BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
57 .xofst = C3_ISP_PHASE_OFFSET_0,
58 .yofst = C3_ISP_PHASE_OFFSET_0,
59 .is_raw = true,
60 }, {
61 .mbus_code = MEDIA_BUS_FMT_SRGGB10_1X10,
62 .pads = BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
63 .xofst = C3_ISP_PHASE_OFFSET_1,
64 .yofst = C3_ISP_PHASE_OFFSET_0,
65 .is_raw = true,
66 }, {
67 .mbus_code = MEDIA_BUS_FMT_SBGGR12_1X12,
68 .pads = BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
69 .xofst = C3_ISP_PHASE_OFFSET_0,
70 .yofst = C3_ISP_PHASE_OFFSET_1,
71 .is_raw = true,
72 }, {
73 .mbus_code = MEDIA_BUS_FMT_SGBRG12_1X12,
74 .pads = BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
75 .xofst = C3_ISP_PHASE_OFFSET_1,
76 .yofst = C3_ISP_PHASE_OFFSET_1,
77 .is_raw = true,
78 }, {
79 .mbus_code = MEDIA_BUS_FMT_SGRBG12_1X12,
80 .pads = BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
81 .xofst = C3_ISP_PHASE_OFFSET_0,
82 .yofst = C3_ISP_PHASE_OFFSET_0,
83 .is_raw = true,
84 }, {
85 .mbus_code = MEDIA_BUS_FMT_SRGGB12_1X12,
86 .pads = BIT(C3_ISP_CORE_PAD_SINK_VIDEO),
87 .xofst = C3_ISP_PHASE_OFFSET_1,
88 .yofst = C3_ISP_PHASE_OFFSET_0,
89 .is_raw = true,
90 }, {
91 .mbus_code = MEDIA_BUS_FMT_SRGGB16_1X16,
92 .pads = BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
93 | BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
94 | BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
95 .xofst = C3_ISP_PHASE_OFFSET_NONE,
96 .yofst = C3_ISP_PHASE_OFFSET_NONE,
97 .is_raw = true,
98 }, {
99 .mbus_code = MEDIA_BUS_FMT_SBGGR16_1X16,
100 .pads = BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
101 | BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
102 | BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
103 .xofst = C3_ISP_PHASE_OFFSET_NONE,
104 .yofst = C3_ISP_PHASE_OFFSET_NONE,
105 .is_raw = true,
106 }, {
107 .mbus_code = MEDIA_BUS_FMT_SGRBG16_1X16,
108 .pads = BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
109 | BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
110 | BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
111 .xofst = C3_ISP_PHASE_OFFSET_NONE,
112 .yofst = C3_ISP_PHASE_OFFSET_NONE,
113 .is_raw = true,
114 }, {
115 .mbus_code = MEDIA_BUS_FMT_SGBRG16_1X16,
116 .pads = BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0)
117 | BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1)
118 | BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
119 .xofst = C3_ISP_PHASE_OFFSET_NONE,
120 .yofst = C3_ISP_PHASE_OFFSET_NONE,
121 .is_raw = true,
122 },
123 /* YUV formats */
124 {
125 .mbus_code = MEDIA_BUS_FMT_YUV10_1X30,
126 .pads = BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_0) |
127 BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_1) |
128 BIT(C3_ISP_CORE_PAD_SOURCE_VIDEO_2),
129 .xofst = C3_ISP_PHASE_OFFSET_NONE,
130 .yofst = C3_ISP_PHASE_OFFSET_NONE,
131 .is_raw = false,
132 },
133 };
134
135 static const struct c3_isp_core_format_info
core_find_format_by_code(u32 code,u32 pad)136 *core_find_format_by_code(u32 code, u32 pad)
137 {
138 for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_core_fmts); i++) {
139 const struct c3_isp_core_format_info *info =
140 &c3_isp_core_fmts[i];
141
142 if (info->mbus_code == code && info->pads & BIT(pad))
143 return info;
144 }
145
146 return NULL;
147 }
148
149 static const struct c3_isp_core_format_info
core_find_format_by_index(u32 index,u32 pad)150 *core_find_format_by_index(u32 index, u32 pad)
151 {
152 for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_core_fmts); i++) {
153 const struct c3_isp_core_format_info *info =
154 &c3_isp_core_fmts[i];
155
156 if (!(info->pads & BIT(pad)))
157 continue;
158
159 if (!index)
160 return info;
161
162 index--;
163 }
164
165 return NULL;
166 }
167
c3_isp_core_enable(struct c3_isp_device * isp)168 static void c3_isp_core_enable(struct c3_isp_device *isp)
169 {
170 c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_END_MASK,
171 ISP_TOP_IRQ_EN_FRM_END_EN);
172 c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_RST_MASK,
173 ISP_TOP_IRQ_EN_FRM_RST_EN);
174
175 /* Enable image data to ISP core */
176 c3_isp_update_bits(isp, ISP_TOP_PATH_SEL, ISP_TOP_PATH_SEL_CORE_MASK,
177 ISP_TOP_PATH_SEL_CORE_MIPI_CORE);
178 }
179
c3_isp_core_disable(struct c3_isp_device * isp)180 static void c3_isp_core_disable(struct c3_isp_device *isp)
181 {
182 /* Disable image data to ISP core */
183 c3_isp_update_bits(isp, ISP_TOP_PATH_SEL, ISP_TOP_PATH_SEL_CORE_MASK,
184 ISP_TOP_PATH_SEL_CORE_CORE_DIS);
185
186 c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_END_MASK,
187 ISP_TOP_IRQ_EN_FRM_END_DIS);
188 c3_isp_update_bits(isp, ISP_TOP_IRQ_EN, ISP_TOP_IRQ_EN_FRM_RST_MASK,
189 ISP_TOP_IRQ_EN_FRM_RST_DIS);
190 }
191
192 /* Set the phase offset of blc, wb and lns */
c3_isp_core_lswb_ofst(struct c3_isp_device * isp,u8 xofst,u8 yofst)193 static void c3_isp_core_lswb_ofst(struct c3_isp_device *isp,
194 u8 xofst, u8 yofst)
195 {
196 c3_isp_update_bits(isp, ISP_LSWB_BLC_PHSOFST,
197 ISP_LSWB_BLC_PHSOFST_HORIZ_OFST_MASK,
198 ISP_LSWB_BLC_PHSOFST_HORIZ_OFST(xofst));
199 c3_isp_update_bits(isp, ISP_LSWB_BLC_PHSOFST,
200 ISP_LSWB_BLC_PHSOFST_VERT_OFST_MASK,
201 ISP_LSWB_BLC_PHSOFST_VERT_OFST(yofst));
202
203 c3_isp_update_bits(isp, ISP_LSWB_WB_PHSOFST,
204 ISP_LSWB_WB_PHSOFST_HORIZ_OFST_MASK,
205 ISP_LSWB_WB_PHSOFST_HORIZ_OFST(xofst));
206 c3_isp_update_bits(isp, ISP_LSWB_WB_PHSOFST,
207 ISP_LSWB_WB_PHSOFST_VERT_OFST_MASK,
208 ISP_LSWB_WB_PHSOFST_VERT_OFST(yofst));
209
210 c3_isp_update_bits(isp, ISP_LSWB_LNS_PHSOFST,
211 ISP_LSWB_LNS_PHSOFST_HORIZ_OFST_MASK,
212 ISP_LSWB_LNS_PHSOFST_HORIZ_OFST(xofst));
213 c3_isp_update_bits(isp, ISP_LSWB_LNS_PHSOFST,
214 ISP_LSWB_LNS_PHSOFST_VERT_OFST_MASK,
215 ISP_LSWB_LNS_PHSOFST_VERT_OFST(yofst));
216 }
217
218 /* Set the phase offset of af, ae and awb */
c3_isp_core_3a_ofst(struct c3_isp_device * isp,u8 xofst,u8 yofst)219 static void c3_isp_core_3a_ofst(struct c3_isp_device *isp,
220 u8 xofst, u8 yofst)
221 {
222 c3_isp_update_bits(isp, ISP_AF_CTRL, ISP_AF_CTRL_HORIZ_OFST_MASK,
223 ISP_AF_CTRL_HORIZ_OFST(xofst));
224 c3_isp_update_bits(isp, ISP_AF_CTRL, ISP_AF_CTRL_VERT_OFST_MASK,
225 ISP_AF_CTRL_VERT_OFST(yofst));
226
227 c3_isp_update_bits(isp, ISP_AE_CTRL, ISP_AE_CTRL_HORIZ_OFST_MASK,
228 ISP_AE_CTRL_HORIZ_OFST(xofst));
229 c3_isp_update_bits(isp, ISP_AE_CTRL, ISP_AE_CTRL_VERT_OFST_MASK,
230 ISP_AE_CTRL_VERT_OFST(yofst));
231
232 c3_isp_update_bits(isp, ISP_AWB_CTRL, ISP_AWB_CTRL_HORIZ_OFST_MASK,
233 ISP_AWB_CTRL_HORIZ_OFST(xofst));
234 c3_isp_update_bits(isp, ISP_AWB_CTRL, ISP_AWB_CTRL_VERT_OFST_MASK,
235 ISP_AWB_CTRL_VERT_OFST(yofst));
236 }
237
238 /* Set the phase offset of demosaic */
c3_isp_core_dms_ofst(struct c3_isp_device * isp,u8 xofst,u8 yofst)239 static void c3_isp_core_dms_ofst(struct c3_isp_device *isp,
240 u8 xofst, u8 yofst)
241 {
242 c3_isp_update_bits(isp, ISP_DMS_COMMON_PARAM0,
243 ISP_DMS_COMMON_PARAM0_HORIZ_PHS_OFST_MASK,
244 ISP_DMS_COMMON_PARAM0_HORIZ_PHS_OFST(xofst));
245 c3_isp_update_bits(isp, ISP_DMS_COMMON_PARAM0,
246 ISP_DMS_COMMON_PARAM0_VERT_PHS_OFST_MASK,
247 ISP_DMS_COMMON_PARAM0_VERT_PHS_OFST(yofst));
248 }
249
c3_isp_core_cfg_format(struct c3_isp_device * isp,struct v4l2_subdev_state * state)250 static void c3_isp_core_cfg_format(struct c3_isp_device *isp,
251 struct v4l2_subdev_state *state)
252 {
253 struct v4l2_mbus_framefmt *fmt;
254 const struct c3_isp_core_format_info *isp_fmt;
255
256 fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SINK_VIDEO);
257 isp_fmt = core_find_format_by_code(fmt->code,
258 C3_ISP_CORE_PAD_SINK_VIDEO);
259
260 c3_isp_write(isp, ISP_TOP_INPUT_SIZE,
261 ISP_TOP_INPUT_SIZE_HORIZ_SIZE(fmt->width) |
262 ISP_TOP_INPUT_SIZE_VERT_SIZE(fmt->height));
263 c3_isp_write(isp, ISP_TOP_FRM_SIZE,
264 ISP_TOP_FRM_SIZE_CORE_HORIZ_SIZE(fmt->width) |
265 ISP_TOP_FRM_SIZE_CORE_VERT_SIZE(fmt->height));
266
267 c3_isp_update_bits(isp, ISP_TOP_HOLD_SIZE,
268 ISP_TOP_HOLD_SIZE_CORE_HORIZ_SIZE_MASK,
269 ISP_TOP_HOLD_SIZE_CORE_HORIZ_SIZE(fmt->width));
270
271 c3_isp_write(isp, ISP_AF_HV_SIZE,
272 ISP_AF_HV_SIZE_GLB_WIN_XSIZE(fmt->width) |
273 ISP_AF_HV_SIZE_GLB_WIN_YSIZE(fmt->height));
274 c3_isp_write(isp, ISP_AE_HV_SIZE,
275 ISP_AE_HV_SIZE_HORIZ_SIZE(fmt->width) |
276 ISP_AE_HV_SIZE_VERT_SIZE(fmt->height));
277 c3_isp_write(isp, ISP_AWB_HV_SIZE,
278 ISP_AWB_HV_SIZE_HORIZ_SIZE(fmt->width) |
279 ISP_AWB_HV_SIZE_VERT_SIZE(fmt->height));
280
281 c3_isp_core_lswb_ofst(isp, isp_fmt->xofst, isp_fmt->yofst);
282 c3_isp_core_3a_ofst(isp, isp_fmt->xofst, isp_fmt->yofst);
283 c3_isp_core_dms_ofst(isp, isp_fmt->xofst, isp_fmt->yofst);
284 }
285
c3_isp_core_streams_ready(struct c3_isp_core * core)286 static bool c3_isp_core_streams_ready(struct c3_isp_core *core)
287 {
288 unsigned int n_links = 0;
289 struct media_link *link;
290
291 for_each_media_entity_data_link(&core->sd.entity, link) {
292 if ((link->source->index == C3_ISP_CORE_PAD_SOURCE_VIDEO_0 ||
293 link->source->index == C3_ISP_CORE_PAD_SOURCE_VIDEO_1 ||
294 link->source->index == C3_ISP_CORE_PAD_SOURCE_VIDEO_2) &&
295 link->flags == MEDIA_LNK_FL_ENABLED)
296 n_links++;
297 }
298
299 return n_links == core->isp->pipe.start_count;
300 }
301
c3_isp_core_enable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)302 static int c3_isp_core_enable_streams(struct v4l2_subdev *sd,
303 struct v4l2_subdev_state *state,
304 u32 pad, u64 streams_mask)
305 {
306 struct c3_isp_core *core = v4l2_get_subdevdata(sd);
307 struct media_pad *sink_pad;
308 struct v4l2_subdev *src_sd;
309 int ret;
310
311 if (!c3_isp_core_streams_ready(core))
312 return 0;
313
314 core->isp->frm_sequence = 0;
315 c3_isp_core_cfg_format(core->isp, state);
316 c3_isp_core_enable(core->isp);
317
318 sink_pad = &core->pads[C3_ISP_CORE_PAD_SINK_VIDEO];
319 core->src_pad = media_pad_remote_pad_unique(sink_pad);
320 if (IS_ERR(core->src_pad)) {
321 dev_dbg(core->isp->dev,
322 "Failed to get source pad for ISP core\n");
323 return -EPIPE;
324 }
325
326 src_sd = media_entity_to_v4l2_subdev(core->src_pad->entity);
327
328 ret = v4l2_subdev_enable_streams(src_sd, core->src_pad->index, BIT(0));
329 if (ret) {
330 c3_isp_core_disable(core->isp);
331 return ret;
332 }
333
334 return 0;
335 }
336
c3_isp_core_disable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)337 static int c3_isp_core_disable_streams(struct v4l2_subdev *sd,
338 struct v4l2_subdev_state *state,
339 u32 pad, u64 streams_mask)
340 {
341 struct c3_isp_core *core = v4l2_get_subdevdata(sd);
342 struct v4l2_subdev *src_sd;
343
344 if (core->isp->pipe.start_count != 1)
345 return 0;
346
347 if (core->src_pad) {
348 src_sd = media_entity_to_v4l2_subdev(core->src_pad->entity);
349 v4l2_subdev_disable_streams(src_sd, core->src_pad->index,
350 BIT(0));
351 }
352 core->src_pad = NULL;
353
354 c3_isp_core_disable(core->isp);
355
356 return 0;
357 }
358
c3_isp_core_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_mbus_code_enum * code)359 static int c3_isp_core_enum_mbus_code(struct v4l2_subdev *sd,
360 struct v4l2_subdev_state *state,
361 struct v4l2_subdev_mbus_code_enum *code)
362 {
363 const struct c3_isp_core_format_info *info;
364
365 switch (code->pad) {
366 case C3_ISP_CORE_PAD_SINK_VIDEO:
367 case C3_ISP_CORE_PAD_SOURCE_VIDEO_0:
368 case C3_ISP_CORE_PAD_SOURCE_VIDEO_1:
369 case C3_ISP_CORE_PAD_SOURCE_VIDEO_2:
370 info = core_find_format_by_index(code->index, code->pad);
371 if (!info)
372 return -EINVAL;
373
374 code->code = info->mbus_code;
375
376 break;
377 case C3_ISP_CORE_PAD_SINK_PARAMS:
378 case C3_ISP_CORE_PAD_SOURCE_STATS:
379 if (code->index)
380 return -EINVAL;
381
382 code->code = MEDIA_BUS_FMT_METADATA_FIXED;
383
384 break;
385 default:
386 return -EINVAL;
387 }
388
389 return 0;
390 }
391
c3_isp_core_set_sink_fmt(struct v4l2_subdev_state * state,struct v4l2_subdev_format * format)392 static void c3_isp_core_set_sink_fmt(struct v4l2_subdev_state *state,
393 struct v4l2_subdev_format *format)
394 {
395 struct v4l2_mbus_framefmt *sink_fmt;
396 struct v4l2_mbus_framefmt *src_fmt;
397 const struct c3_isp_core_format_info *isp_fmt;
398
399 sink_fmt = v4l2_subdev_state_get_format(state, format->pad);
400
401 isp_fmt = core_find_format_by_code(format->format.code, format->pad);
402 if (!isp_fmt)
403 sink_fmt->code = C3_ISP_CORE_DEF_SINK_PAD_FMT;
404 else
405 sink_fmt->code = format->format.code;
406
407 sink_fmt->width = clamp_t(u32, format->format.width,
408 C3_ISP_MIN_WIDTH, C3_ISP_MAX_WIDTH);
409 sink_fmt->height = clamp_t(u32, format->format.height,
410 C3_ISP_MIN_HEIGHT, C3_ISP_MAX_HEIGHT);
411 sink_fmt->field = V4L2_FIELD_NONE;
412 sink_fmt->colorspace = V4L2_COLORSPACE_RAW;
413 sink_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
414 sink_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
415 sink_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
416
417 for (unsigned int i = C3_ISP_CORE_PAD_SOURCE_VIDEO_0;
418 i < C3_ISP_CORE_PAD_MAX; i++) {
419 src_fmt = v4l2_subdev_state_get_format(state, i);
420
421 src_fmt->width = sink_fmt->width;
422 src_fmt->height = sink_fmt->height;
423 }
424
425 format->format = *sink_fmt;
426 }
427
c3_isp_core_set_source_fmt(struct v4l2_subdev_state * state,struct v4l2_subdev_format * format)428 static void c3_isp_core_set_source_fmt(struct v4l2_subdev_state *state,
429 struct v4l2_subdev_format *format)
430 {
431 const struct c3_isp_core_format_info *isp_fmt;
432 struct v4l2_mbus_framefmt *src_fmt;
433 struct v4l2_mbus_framefmt *sink_fmt;
434
435 sink_fmt = v4l2_subdev_state_get_format(state,
436 C3_ISP_CORE_PAD_SINK_VIDEO);
437 src_fmt = v4l2_subdev_state_get_format(state, format->pad);
438
439 isp_fmt = core_find_format_by_code(format->format.code, format->pad);
440 if (!isp_fmt)
441 src_fmt->code = C3_ISP_CORE_DEF_SRC_PAD_FMT;
442 else
443 src_fmt->code = format->format.code;
444
445 src_fmt->width = sink_fmt->width;
446 src_fmt->height = sink_fmt->height;
447 src_fmt->field = V4L2_FIELD_NONE;
448 src_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
449
450 if (isp_fmt && isp_fmt->is_raw) {
451 src_fmt->colorspace = V4L2_COLORSPACE_RAW;
452 src_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
453 src_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
454 } else {
455 src_fmt->colorspace = V4L2_COLORSPACE_SRGB;
456 src_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
457 src_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
458 }
459
460 format->format = *src_fmt;
461 }
462
c3_isp_core_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_format * format)463 static int c3_isp_core_set_fmt(struct v4l2_subdev *sd,
464 struct v4l2_subdev_state *state,
465 struct v4l2_subdev_format *format)
466 {
467 if (format->pad == C3_ISP_CORE_PAD_SINK_VIDEO)
468 c3_isp_core_set_sink_fmt(state, format);
469 else if (format->pad == C3_ISP_CORE_PAD_SOURCE_VIDEO_0 ||
470 format->pad == C3_ISP_CORE_PAD_SOURCE_VIDEO_1 ||
471 format->pad == C3_ISP_CORE_PAD_SOURCE_VIDEO_2)
472 c3_isp_core_set_source_fmt(state, format);
473 else
474 format->format =
475 *v4l2_subdev_state_get_format(state, format->pad);
476
477 return 0;
478 }
479
c3_isp_core_init_state(struct v4l2_subdev * sd,struct v4l2_subdev_state * state)480 static int c3_isp_core_init_state(struct v4l2_subdev *sd,
481 struct v4l2_subdev_state *state)
482 {
483 struct v4l2_mbus_framefmt *fmt;
484
485 /* Video sink pad */
486 fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SINK_VIDEO);
487 fmt->width = C3_ISP_DEFAULT_WIDTH;
488 fmt->height = C3_ISP_DEFAULT_HEIGHT;
489 fmt->field = V4L2_FIELD_NONE;
490 fmt->code = C3_ISP_CORE_DEF_SINK_PAD_FMT;
491 fmt->colorspace = V4L2_COLORSPACE_RAW;
492 fmt->xfer_func = V4L2_XFER_FUNC_NONE;
493 fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
494 fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
495
496 /* Video source pad */
497 for (unsigned int i = C3_ISP_CORE_PAD_SOURCE_VIDEO_0;
498 i < C3_ISP_CORE_PAD_MAX; i++) {
499 fmt = v4l2_subdev_state_get_format(state, i);
500 fmt->width = C3_ISP_DEFAULT_WIDTH;
501 fmt->height = C3_ISP_DEFAULT_HEIGHT;
502 fmt->field = V4L2_FIELD_NONE;
503 fmt->code = C3_ISP_CORE_DEF_SRC_PAD_FMT;
504 fmt->colorspace = V4L2_COLORSPACE_SRGB;
505 fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
506 fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
507 fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
508 }
509
510 /* Parameters pad */
511 fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SINK_PARAMS);
512 fmt->width = 0;
513 fmt->height = 0;
514 fmt->field = V4L2_FIELD_NONE;
515 fmt->code = MEDIA_BUS_FMT_METADATA_FIXED;
516
517 /* Statistics pad */
518 fmt = v4l2_subdev_state_get_format(state, C3_ISP_CORE_PAD_SOURCE_STATS);
519 fmt->width = 0;
520 fmt->height = 0;
521 fmt->field = V4L2_FIELD_NONE;
522 fmt->code = MEDIA_BUS_FMT_METADATA_FIXED;
523
524 return 0;
525 }
526
c3_isp_core_subscribe_event(struct v4l2_subdev * sd,struct v4l2_fh * fh,struct v4l2_event_subscription * sub)527 static int c3_isp_core_subscribe_event(struct v4l2_subdev *sd,
528 struct v4l2_fh *fh,
529 struct v4l2_event_subscription *sub)
530 {
531 if (sub->type != V4L2_EVENT_FRAME_SYNC)
532 return -EINVAL;
533
534 /* V4L2_EVENT_FRAME_SYNC doesn't need id, so should set 0 */
535 if (sub->id != 0)
536 return -EINVAL;
537
538 return v4l2_event_subscribe(fh, sub, 0, NULL);
539 }
540
541 static const struct v4l2_subdev_pad_ops c3_isp_core_pad_ops = {
542 .enum_mbus_code = c3_isp_core_enum_mbus_code,
543 .get_fmt = v4l2_subdev_get_fmt,
544 .set_fmt = c3_isp_core_set_fmt,
545 .enable_streams = c3_isp_core_enable_streams,
546 .disable_streams = c3_isp_core_disable_streams,
547 };
548
549 static const struct v4l2_subdev_core_ops c3_isp_core_core_ops = {
550 .subscribe_event = c3_isp_core_subscribe_event,
551 .unsubscribe_event = v4l2_event_subdev_unsubscribe,
552 };
553
554 static const struct v4l2_subdev_ops c3_isp_core_subdev_ops = {
555 .core = &c3_isp_core_core_ops,
556 .pad = &c3_isp_core_pad_ops,
557 };
558
559 static const struct v4l2_subdev_internal_ops c3_isp_core_internal_ops = {
560 .init_state = c3_isp_core_init_state,
561 };
562
c3_isp_core_link_validate(struct media_link * link)563 static int c3_isp_core_link_validate(struct media_link *link)
564 {
565 if (link->sink->index == C3_ISP_CORE_PAD_SINK_PARAMS)
566 return 0;
567
568 return v4l2_subdev_link_validate(link);
569 }
570
571 /* Media entity operations */
572 static const struct media_entity_operations c3_isp_core_entity_ops = {
573 .link_validate = c3_isp_core_link_validate,
574 };
575
c3_isp_core_queue_sof(struct c3_isp_device * isp)576 void c3_isp_core_queue_sof(struct c3_isp_device *isp)
577 {
578 struct v4l2_event event = {
579 .type = V4L2_EVENT_FRAME_SYNC,
580 };
581
582 event.u.frame_sync.frame_sequence = isp->frm_sequence;
583 v4l2_event_queue(isp->core.sd.devnode, &event);
584 }
585
c3_isp_core_register(struct c3_isp_device * isp)586 int c3_isp_core_register(struct c3_isp_device *isp)
587 {
588 struct c3_isp_core *core = &isp->core;
589 struct v4l2_subdev *sd = &core->sd;
590 int ret;
591
592 v4l2_subdev_init(sd, &c3_isp_core_subdev_ops);
593 sd->owner = THIS_MODULE;
594 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
595 sd->internal_ops = &c3_isp_core_internal_ops;
596 snprintf(sd->name, sizeof(sd->name), "%s", C3_ISP_CORE_SUBDEV_NAME);
597
598 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
599 sd->entity.ops = &c3_isp_core_entity_ops;
600
601 core->isp = isp;
602 sd->dev = isp->dev;
603 v4l2_set_subdevdata(sd, core);
604
605 core->pads[C3_ISP_CORE_PAD_SINK_VIDEO].flags = MEDIA_PAD_FL_SINK;
606 core->pads[C3_ISP_CORE_PAD_SINK_PARAMS].flags = MEDIA_PAD_FL_SINK;
607 core->pads[C3_ISP_CORE_PAD_SOURCE_STATS].flags = MEDIA_PAD_FL_SOURCE;
608 core->pads[C3_ISP_CORE_PAD_SOURCE_VIDEO_0].flags = MEDIA_PAD_FL_SOURCE;
609 core->pads[C3_ISP_CORE_PAD_SOURCE_VIDEO_1].flags = MEDIA_PAD_FL_SOURCE;
610 core->pads[C3_ISP_CORE_PAD_SOURCE_VIDEO_2].flags = MEDIA_PAD_FL_SOURCE;
611 ret = media_entity_pads_init(&sd->entity, C3_ISP_CORE_PAD_MAX,
612 core->pads);
613 if (ret)
614 return ret;
615
616 ret = v4l2_subdev_init_finalize(sd);
617 if (ret)
618 goto err_entity_cleanup;
619
620 ret = v4l2_device_register_subdev(&isp->v4l2_dev, sd);
621 if (ret)
622 goto err_subdev_cleanup;
623
624 return 0;
625
626 err_subdev_cleanup:
627 v4l2_subdev_cleanup(sd);
628 err_entity_cleanup:
629 media_entity_cleanup(&sd->entity);
630 return ret;
631 }
632
c3_isp_core_unregister(struct c3_isp_device * isp)633 void c3_isp_core_unregister(struct c3_isp_device *isp)
634 {
635 struct c3_isp_core *core = &isp->core;
636 struct v4l2_subdev *sd = &core->sd;
637
638 v4l2_device_unregister_subdev(sd);
639 v4l2_subdev_cleanup(sd);
640 media_entity_cleanup(&sd->entity);
641 }
642