1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * stf_isp.c
4 *
5 * StarFive Camera Subsystem - ISP Module
6 *
7 * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
8 */
9 #include <media/v4l2-rect.h>
10
11 #include "stf-camss.h"
12
13 static int isp_set_selection(struct v4l2_subdev *sd,
14 struct v4l2_subdev_state *state,
15 struct v4l2_subdev_selection *sel);
16
17 static const struct stf_isp_format isp_formats_sink[] = {
18 { MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
19 { MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
20 { MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
21 { MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
22 };
23
24 static const struct stf_isp_format isp_formats_source[] = {
25 { MEDIA_BUS_FMT_YUYV8_1_5X8, 8 },
26 };
27
28 static const struct stf_isp_format_table isp_formats_st7110[] = {
29 { isp_formats_sink, ARRAY_SIZE(isp_formats_sink) },
30 { isp_formats_source, ARRAY_SIZE(isp_formats_source) },
31 };
32
33 static const struct stf_isp_format *
stf_g_fmt_by_mcode(const struct stf_isp_format_table * fmt_table,u32 mcode)34 stf_g_fmt_by_mcode(const struct stf_isp_format_table *fmt_table, u32 mcode)
35 {
36 unsigned int i;
37
38 for (i = 0; i < fmt_table->nfmts; i++) {
39 if (fmt_table->fmts[i].code == mcode)
40 return &fmt_table->fmts[i];
41 }
42
43 return NULL;
44 }
45
stf_isp_init(struct stfcamss * stfcamss)46 int stf_isp_init(struct stfcamss *stfcamss)
47 {
48 struct stf_isp_dev *isp_dev = &stfcamss->isp_dev;
49
50 isp_dev->stfcamss = stfcamss;
51 isp_dev->formats = isp_formats_st7110;
52 isp_dev->nformats = ARRAY_SIZE(isp_formats_st7110);
53 isp_dev->current_fmt = &isp_formats_source[0];
54
55 return 0;
56 }
57
isp_set_stream(struct v4l2_subdev * sd,int enable)58 static int isp_set_stream(struct v4l2_subdev *sd, int enable)
59 {
60 struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
61 struct v4l2_subdev_state *sd_state;
62 struct v4l2_mbus_framefmt *fmt;
63 struct v4l2_rect *crop;
64
65 sd_state = v4l2_subdev_lock_and_get_active_state(sd);
66 fmt = v4l2_subdev_state_get_format(sd_state, STF_ISP_PAD_SINK);
67 crop = v4l2_subdev_state_get_crop(sd_state, STF_ISP_PAD_SRC);
68
69 if (enable) {
70 stf_isp_reset(isp_dev);
71 stf_isp_init_cfg(isp_dev);
72 stf_isp_settings(isp_dev, crop, fmt->code);
73 stf_isp_stream_set(isp_dev);
74 }
75
76 v4l2_subdev_call(isp_dev->source_subdev, video, s_stream, enable);
77
78 v4l2_subdev_unlock_state(sd_state);
79 return 0;
80 }
81
isp_try_format(struct stf_isp_dev * isp_dev,struct v4l2_subdev_state * state,unsigned int pad,struct v4l2_mbus_framefmt * fmt)82 static void isp_try_format(struct stf_isp_dev *isp_dev,
83 struct v4l2_subdev_state *state,
84 unsigned int pad,
85 struct v4l2_mbus_framefmt *fmt)
86 {
87 const struct stf_isp_format_table *formats;
88
89 if (pad >= STF_ISP_PAD_MAX) {
90 fmt->colorspace = V4L2_COLORSPACE_SRGB;
91 return;
92 }
93
94 formats = &isp_dev->formats[pad];
95
96 fmt->width = clamp_t(u32, fmt->width, STFCAMSS_FRAME_MIN_WIDTH,
97 STFCAMSS_FRAME_MAX_WIDTH);
98 fmt->height = clamp_t(u32, fmt->height, STFCAMSS_FRAME_MIN_HEIGHT,
99 STFCAMSS_FRAME_MAX_HEIGHT);
100 fmt->height &= ~0x1;
101 fmt->field = V4L2_FIELD_NONE;
102 fmt->colorspace = V4L2_COLORSPACE_SRGB;
103 fmt->flags = 0;
104
105 if (!stf_g_fmt_by_mcode(formats, fmt->code))
106 fmt->code = formats->fmts[0].code;
107 }
108
isp_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_mbus_code_enum * code)109 static int isp_enum_mbus_code(struct v4l2_subdev *sd,
110 struct v4l2_subdev_state *state,
111 struct v4l2_subdev_mbus_code_enum *code)
112 {
113 struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
114 const struct stf_isp_format_table *formats;
115
116 if (code->pad == STF_ISP_PAD_SINK) {
117 if (code->index >= ARRAY_SIZE(isp_formats_sink))
118 return -EINVAL;
119
120 formats = &isp_dev->formats[code->pad];
121 code->code = formats->fmts[code->index].code;
122 } else {
123 struct v4l2_mbus_framefmt *sink_fmt;
124
125 if (code->index >= ARRAY_SIZE(isp_formats_source))
126 return -EINVAL;
127
128 sink_fmt = v4l2_subdev_state_get_format(state,
129 STF_ISP_PAD_SRC);
130
131 code->code = sink_fmt->code;
132 if (!code->code)
133 return -EINVAL;
134 }
135 code->flags = 0;
136
137 return 0;
138 }
139
isp_set_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_format * fmt)140 static int isp_set_format(struct v4l2_subdev *sd,
141 struct v4l2_subdev_state *state,
142 struct v4l2_subdev_format *fmt)
143 {
144 struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
145 struct v4l2_mbus_framefmt *format;
146
147 format = v4l2_subdev_state_get_format(state, fmt->pad);
148 if (!format)
149 return -EINVAL;
150
151 isp_try_format(isp_dev, state, fmt->pad, &fmt->format);
152 *format = fmt->format;
153
154 isp_dev->current_fmt = stf_g_fmt_by_mcode(&isp_dev->formats[fmt->pad],
155 fmt->format.code);
156
157 /* Propagate to in crop */
158 if (fmt->pad == STF_ISP_PAD_SINK) {
159 struct v4l2_subdev_selection sel = { 0 };
160
161 /* Reset sink pad compose selection */
162 sel.which = fmt->which;
163 sel.pad = STF_ISP_PAD_SINK;
164 sel.target = V4L2_SEL_TGT_CROP;
165 sel.r.width = fmt->format.width;
166 sel.r.height = fmt->format.height;
167 isp_set_selection(sd, state, &sel);
168 }
169
170 return 0;
171 }
172
173 static const struct v4l2_rect stf_frame_min_crop = {
174 .width = STFCAMSS_FRAME_MIN_WIDTH,
175 .height = STFCAMSS_FRAME_MIN_HEIGHT,
176 .top = 0,
177 .left = 0,
178 };
179
isp_try_crop(struct stf_isp_dev * isp_dev,struct v4l2_subdev_state * state,struct v4l2_rect * crop)180 static void isp_try_crop(struct stf_isp_dev *isp_dev,
181 struct v4l2_subdev_state *state,
182 struct v4l2_rect *crop)
183 {
184 struct v4l2_mbus_framefmt *fmt =
185 v4l2_subdev_state_get_format(state, STF_ISP_PAD_SINK);
186
187 const struct v4l2_rect bounds = {
188 .width = fmt->width,
189 .height = fmt->height,
190 .left = 0,
191 .top = 0,
192 };
193
194 v4l2_rect_set_min_size(crop, &stf_frame_min_crop);
195 v4l2_rect_map_inside(crop, &bounds);
196 }
197
isp_get_selection(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_selection * sel)198 static int isp_get_selection(struct v4l2_subdev *sd,
199 struct v4l2_subdev_state *state,
200 struct v4l2_subdev_selection *sel)
201 {
202 struct v4l2_subdev_format fmt = { 0 };
203 struct v4l2_rect *rect;
204
205 switch (sel->target) {
206 case V4L2_SEL_TGT_CROP_BOUNDS:
207 if (sel->pad == STF_ISP_PAD_SINK) {
208 fmt.format = *v4l2_subdev_state_get_format(state,
209 sel->pad);
210 sel->r.left = 0;
211 sel->r.top = 0;
212 sel->r.width = fmt.format.width;
213 sel->r.height = fmt.format.height;
214 } else if (sel->pad == STF_ISP_PAD_SRC) {
215 rect = v4l2_subdev_state_get_crop(state, sel->pad);
216 sel->r = *rect;
217 }
218 break;
219
220 case V4L2_SEL_TGT_CROP:
221 rect = v4l2_subdev_state_get_crop(state, sel->pad);
222 if (!rect)
223 return -EINVAL;
224
225 sel->r = *rect;
226 break;
227
228 default:
229 return -EINVAL;
230 }
231
232 return 0;
233 }
234
isp_set_selection(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_selection * sel)235 static int isp_set_selection(struct v4l2_subdev *sd,
236 struct v4l2_subdev_state *state,
237 struct v4l2_subdev_selection *sel)
238 {
239 struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
240 struct v4l2_rect *rect;
241
242 if (sel->target != V4L2_SEL_TGT_CROP)
243 return -EINVAL;
244
245 if (sel->target == V4L2_SEL_TGT_CROP &&
246 sel->pad == STF_ISP_PAD_SINK) {
247 struct v4l2_subdev_selection crop = { 0 };
248
249 rect = v4l2_subdev_state_get_crop(state, sel->pad);
250 if (!rect)
251 return -EINVAL;
252
253 isp_try_crop(isp_dev, state, &sel->r);
254 *rect = sel->r;
255
256 /* Reset source crop selection */
257 crop.which = sel->which;
258 crop.pad = STF_ISP_PAD_SRC;
259 crop.target = V4L2_SEL_TGT_CROP;
260 crop.r = *rect;
261 isp_set_selection(sd, state, &crop);
262 } else if (sel->target == V4L2_SEL_TGT_CROP &&
263 sel->pad == STF_ISP_PAD_SRC) {
264 struct v4l2_subdev_format fmt = { 0 };
265
266 rect = v4l2_subdev_state_get_crop(state, sel->pad);
267 if (!rect)
268 return -EINVAL;
269
270 isp_try_crop(isp_dev, state, &sel->r);
271 *rect = sel->r;
272
273 /* Reset source pad format width and height */
274 fmt.which = sel->which;
275 fmt.pad = STF_ISP_PAD_SRC;
276 fmt.format.width = rect->width;
277 fmt.format.height = rect->height;
278 isp_set_format(sd, state, &fmt);
279 }
280
281 dev_dbg(isp_dev->stfcamss->dev, "pad: %d sel(%d,%d)/%dx%d\n",
282 sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height);
283
284 return 0;
285 }
286
isp_init_formats(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state)287 static int isp_init_formats(struct v4l2_subdev *sd,
288 struct v4l2_subdev_state *sd_state)
289 {
290 struct v4l2_subdev_format format = {
291 .pad = STF_ISP_PAD_SINK,
292 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
293 .format = {
294 .code = MEDIA_BUS_FMT_SRGGB10_1X10,
295 .width = 1920,
296 .height = 1080
297 }
298 };
299
300 return isp_set_format(sd, sd_state, &format);
301 }
302
303 static const struct v4l2_subdev_video_ops isp_video_ops = {
304 .s_stream = isp_set_stream,
305 };
306
307 static const struct v4l2_subdev_pad_ops isp_pad_ops = {
308 .enum_mbus_code = isp_enum_mbus_code,
309 .get_fmt = v4l2_subdev_get_fmt,
310 .set_fmt = isp_set_format,
311 .get_selection = isp_get_selection,
312 .set_selection = isp_set_selection,
313 };
314
315 static const struct v4l2_subdev_ops isp_v4l2_ops = {
316 .video = &isp_video_ops,
317 .pad = &isp_pad_ops,
318 };
319
320 static const struct v4l2_subdev_internal_ops isp_internal_ops = {
321 .init_state = isp_init_formats,
322 };
323
324 static const struct media_entity_operations isp_media_ops = {
325 .link_validate = v4l2_subdev_link_validate,
326 };
327
stf_isp_register(struct stf_isp_dev * isp_dev,struct v4l2_device * v4l2_dev)328 int stf_isp_register(struct stf_isp_dev *isp_dev, struct v4l2_device *v4l2_dev)
329 {
330 struct v4l2_subdev *sd = &isp_dev->subdev;
331 struct media_pad *pads = isp_dev->pads;
332 int ret;
333
334 v4l2_subdev_init(sd, &isp_v4l2_ops);
335 sd->internal_ops = &isp_internal_ops;
336 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
337 snprintf(sd->name, ARRAY_SIZE(sd->name), "stf_isp");
338 v4l2_set_subdevdata(sd, isp_dev);
339
340 pads[STF_ISP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
341 pads[STF_ISP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
342
343 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_ISP;
344 sd->entity.ops = &isp_media_ops;
345 ret = media_entity_pads_init(&sd->entity, STF_ISP_PAD_MAX, pads);
346 if (ret) {
347 dev_err(isp_dev->stfcamss->dev,
348 "Failed to init media entity: %d\n", ret);
349 return ret;
350 }
351
352 ret = v4l2_subdev_init_finalize(sd);
353 if (ret)
354 goto err_entity_cleanup;
355
356 ret = v4l2_device_register_subdev(v4l2_dev, sd);
357 if (ret) {
358 dev_err(isp_dev->stfcamss->dev,
359 "Failed to register subdev: %d\n", ret);
360 goto err_subdev_cleanup;
361 }
362
363 return 0;
364
365 err_subdev_cleanup:
366 v4l2_subdev_cleanup(sd);
367 err_entity_cleanup:
368 media_entity_cleanup(&sd->entity);
369 return ret;
370 }
371
stf_isp_unregister(struct stf_isp_dev * isp_dev)372 int stf_isp_unregister(struct stf_isp_dev *isp_dev)
373 {
374 v4l2_device_unregister_subdev(&isp_dev->subdev);
375 v4l2_subdev_cleanup(&isp_dev->subdev);
376 media_entity_cleanup(&isp_dev->subdev.entity);
377
378 return 0;
379 }
380