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