stf-isp.c

  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
 13static int isp_set_selection(struct v4l2_subdev *sd,
 14			     struct v4l2_subdev_state *state,
 15			     struct v4l2_subdev_selection *sel);
 16
 17static 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
 24static const struct stf_isp_format isp_formats_source[] = {
 25	{ MEDIA_BUS_FMT_YUYV8_1_5X8, 8 },
 26};
 27
 28static 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
 33static const struct stf_isp_format *
 34stf_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
 46int 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
 58static 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
 82static 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
109static 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
140static 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
173static 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
180static 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
198static 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
235static 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)/%ux%u\n",
282		sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height);
283
284	return 0;
285}
286
287static 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
303static const struct v4l2_subdev_video_ops isp_video_ops = {
304	.s_stream = isp_set_stream,
305};
306
307static 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
315static const struct v4l2_subdev_ops isp_v4l2_ops = {
316	.video = &isp_video_ops,
317	.pad = &isp_pad_ops,
318};
319
320static const struct v4l2_subdev_internal_ops isp_internal_ops = {
321	.init_state = isp_init_formats,
322};
323
324static const struct media_entity_operations isp_media_ops = {
325	.link_validate = v4l2_subdev_link_validate,
326};
327
328int 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
365err_subdev_cleanup:
366	v4l2_subdev_cleanup(sd);
367err_entity_cleanup:
368	media_entity_cleanup(&sd->entity);
369	return ret;
370}
371
372int 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}