| // SPDX-License-Identifier: GPL-2.0 |
| /* |
| * stf_isp.c |
| * |
| * StarFive Camera Subsystem - ISP Module |
| * |
| * Copyright (C) 2021-2023 StarFive Technology Co., Ltd. |
| */ |
| #include <media/v4l2-rect.h> |
| |
| #include "stf-camss.h" |
| |
| #define SINK_FORMATS_INDEX 0 |
| #define SOURCE_FORMATS_INDEX 1 |
| |
| static int isp_set_selection(struct v4l2_subdev *sd, |
| struct v4l2_subdev_state *state, |
| struct v4l2_subdev_selection *sel); |
| |
| static const struct stf_isp_format isp_formats_sink[] = { |
| { MEDIA_BUS_FMT_SRGGB10_1X10, 10 }, |
| { MEDIA_BUS_FMT_SGRBG10_1X10, 10 }, |
| { MEDIA_BUS_FMT_SGBRG10_1X10, 10 }, |
| { MEDIA_BUS_FMT_SBGGR10_1X10, 10 }, |
| }; |
| |
| static const struct stf_isp_format isp_formats_source[] = { |
| { MEDIA_BUS_FMT_YUYV8_1_5X8, 8 }, |
| }; |
| |
| static const struct stf_isp_format_table isp_formats_st7110[] = { |
| { isp_formats_sink, ARRAY_SIZE(isp_formats_sink) }, |
| { isp_formats_source, ARRAY_SIZE(isp_formats_source) }, |
| }; |
| |
| static const struct stf_isp_format * |
| stf_g_fmt_by_mcode(const struct stf_isp_format_table *fmt_table, u32 mcode) |
| { |
| unsigned int i; |
| |
| for (i = 0; i < fmt_table->nfmts; i++) { |
| if (fmt_table->fmts[i].code == mcode) |
| return &fmt_table->fmts[i]; |
| } |
| |
| return NULL; |
| } |
| |
| int stf_isp_init(struct stfcamss *stfcamss) |
| { |
| struct stf_isp_dev *isp_dev = &stfcamss->isp_dev; |
| |
| isp_dev->stfcamss = stfcamss; |
| isp_dev->formats = isp_formats_st7110; |
| isp_dev->nformats = ARRAY_SIZE(isp_formats_st7110); |
| isp_dev->current_fmt = &isp_formats_source[0]; |
| |
| return 0; |
| } |
| |
| static int isp_set_stream(struct v4l2_subdev *sd, int enable) |
| { |
| struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd); |
| struct v4l2_subdev_state *sd_state; |
| struct v4l2_mbus_framefmt *fmt; |
| struct v4l2_rect *crop; |
| |
| sd_state = v4l2_subdev_lock_and_get_active_state(sd); |
| fmt = v4l2_subdev_state_get_format(sd_state, STF_ISP_PAD_SINK); |
| crop = v4l2_subdev_state_get_crop(sd_state, STF_ISP_PAD_SRC); |
| |
| if (enable) { |
| stf_isp_reset(isp_dev); |
| stf_isp_init_cfg(isp_dev); |
| stf_isp_settings(isp_dev, crop, fmt->code); |
| stf_isp_stream_set(isp_dev); |
| } |
| |
| v4l2_subdev_call(isp_dev->source_subdev, video, s_stream, enable); |
| |
| v4l2_subdev_unlock_state(sd_state); |
| return 0; |
| } |
| |
| static void isp_try_format(struct stf_isp_dev *isp_dev, |
| struct v4l2_subdev_state *state, |
| unsigned int pad, |
| struct v4l2_mbus_framefmt *fmt) |
| { |
| const struct stf_isp_format_table *formats; |
| |
| if (pad >= STF_ISP_PAD_MAX) { |
| fmt->colorspace = V4L2_COLORSPACE_SRGB; |
| return; |
| } |
| |
| if (pad == STF_ISP_PAD_SINK) |
| formats = &isp_dev->formats[SINK_FORMATS_INDEX]; |
| else if (pad == STF_ISP_PAD_SRC) |
| formats = &isp_dev->formats[SOURCE_FORMATS_INDEX]; |
| |
| fmt->width = clamp_t(u32, fmt->width, STFCAMSS_FRAME_MIN_WIDTH, |
| STFCAMSS_FRAME_MAX_WIDTH); |
| fmt->height = clamp_t(u32, fmt->height, STFCAMSS_FRAME_MIN_HEIGHT, |
| STFCAMSS_FRAME_MAX_HEIGHT); |
| fmt->height &= ~0x1; |
| fmt->field = V4L2_FIELD_NONE; |
| fmt->colorspace = V4L2_COLORSPACE_SRGB; |
| fmt->flags = 0; |
| |
| if (!stf_g_fmt_by_mcode(formats, fmt->code)) |
| fmt->code = formats->fmts[0].code; |
| } |
| |
| static int isp_enum_mbus_code(struct v4l2_subdev *sd, |
| struct v4l2_subdev_state *state, |
| struct v4l2_subdev_mbus_code_enum *code) |
| { |
| struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd); |
| const struct stf_isp_format_table *formats; |
| |
| if (code->pad == STF_ISP_PAD_SINK) { |
| if (code->index >= ARRAY_SIZE(isp_formats_sink)) |
| return -EINVAL; |
| |
| formats = &isp_dev->formats[SINK_FORMATS_INDEX]; |
| code->code = formats->fmts[code->index].code; |
| } else { |
| struct v4l2_mbus_framefmt *sink_fmt; |
| |
| if (code->index >= ARRAY_SIZE(isp_formats_source)) |
| return -EINVAL; |
| |
| sink_fmt = v4l2_subdev_state_get_format(state, |
| STF_ISP_PAD_SRC); |
| |
| code->code = sink_fmt->code; |
| if (!code->code) |
| return -EINVAL; |
| } |
| code->flags = 0; |
| |
| return 0; |
| } |
| |
| static int isp_set_format(struct v4l2_subdev *sd, |
| struct v4l2_subdev_state *state, |
| struct v4l2_subdev_format *fmt) |
| { |
| struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd); |
| struct v4l2_mbus_framefmt *format; |
| |
| format = v4l2_subdev_state_get_format(state, fmt->pad); |
| if (!format) |
| return -EINVAL; |
| |
| isp_try_format(isp_dev, state, fmt->pad, &fmt->format); |
| *format = fmt->format; |
| |
| isp_dev->current_fmt = stf_g_fmt_by_mcode(&isp_dev->formats[fmt->pad], |
| fmt->format.code); |
| |
| /* Propagate to in crop */ |
| if (fmt->pad == STF_ISP_PAD_SINK) { |
| struct v4l2_subdev_selection sel = { 0 }; |
| |
| /* Reset sink pad compose selection */ |
| sel.which = fmt->which; |
| sel.pad = STF_ISP_PAD_SINK; |
| sel.target = V4L2_SEL_TGT_CROP; |
| sel.r.width = fmt->format.width; |
| sel.r.height = fmt->format.height; |
| isp_set_selection(sd, state, &sel); |
| } |
| |
| return 0; |
| } |
| |
| static const struct v4l2_rect stf_frame_min_crop = { |
| .width = STFCAMSS_FRAME_MIN_WIDTH, |
| .height = STFCAMSS_FRAME_MIN_HEIGHT, |
| .top = 0, |
| .left = 0, |
| }; |
| |
| static void isp_try_crop(struct stf_isp_dev *isp_dev, |
| struct v4l2_subdev_state *state, |
| struct v4l2_rect *crop) |
| { |
| struct v4l2_mbus_framefmt *fmt = |
| v4l2_subdev_state_get_format(state, STF_ISP_PAD_SINK); |
| |
| const struct v4l2_rect bounds = { |
| .width = fmt->width, |
| .height = fmt->height, |
| .left = 0, |
| .top = 0, |
| }; |
| |
| v4l2_rect_set_min_size(crop, &stf_frame_min_crop); |
| v4l2_rect_map_inside(crop, &bounds); |
| } |
| |
| static int isp_get_selection(struct v4l2_subdev *sd, |
| struct v4l2_subdev_state *state, |
| struct v4l2_subdev_selection *sel) |
| { |
| struct v4l2_subdev_format fmt = { 0 }; |
| struct v4l2_rect *rect; |
| |
| switch (sel->target) { |
| case V4L2_SEL_TGT_CROP_BOUNDS: |
| if (sel->pad == STF_ISP_PAD_SINK) { |
| fmt.format = *v4l2_subdev_state_get_format(state, |
| sel->pad); |
| sel->r.left = 0; |
| sel->r.top = 0; |
| sel->r.width = fmt.format.width; |
| sel->r.height = fmt.format.height; |
| } else if (sel->pad == STF_ISP_PAD_SRC) { |
| rect = v4l2_subdev_state_get_crop(state, sel->pad); |
| sel->r = *rect; |
| } |
| break; |
| |
| case V4L2_SEL_TGT_CROP: |
| rect = v4l2_subdev_state_get_crop(state, sel->pad); |
| if (!rect) |
| return -EINVAL; |
| |
| sel->r = *rect; |
| break; |
| |
| default: |
| return -EINVAL; |
| } |
| |
| return 0; |
| } |
| |
| static int isp_set_selection(struct v4l2_subdev *sd, |
| struct v4l2_subdev_state *state, |
| struct v4l2_subdev_selection *sel) |
| { |
| struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd); |
| struct v4l2_rect *rect; |
| |
| if (sel->target != V4L2_SEL_TGT_CROP) |
| return -EINVAL; |
| |
| if (sel->target == V4L2_SEL_TGT_CROP && |
| sel->pad == STF_ISP_PAD_SINK) { |
| struct v4l2_subdev_selection crop = { 0 }; |
| |
| rect = v4l2_subdev_state_get_crop(state, sel->pad); |
| if (!rect) |
| return -EINVAL; |
| |
| isp_try_crop(isp_dev, state, &sel->r); |
| *rect = sel->r; |
| |
| /* Reset source crop selection */ |
| crop.which = sel->which; |
| crop.pad = STF_ISP_PAD_SRC; |
| crop.target = V4L2_SEL_TGT_CROP; |
| crop.r = *rect; |
| isp_set_selection(sd, state, &crop); |
| } else if (sel->target == V4L2_SEL_TGT_CROP && |
| sel->pad == STF_ISP_PAD_SRC) { |
| struct v4l2_subdev_format fmt = { 0 }; |
| |
| rect = v4l2_subdev_state_get_crop(state, sel->pad); |
| if (!rect) |
| return -EINVAL; |
| |
| isp_try_crop(isp_dev, state, &sel->r); |
| *rect = sel->r; |
| |
| /* Reset source pad format width and height */ |
| fmt.which = sel->which; |
| fmt.pad = STF_ISP_PAD_SRC; |
| fmt.format.width = rect->width; |
| fmt.format.height = rect->height; |
| isp_set_format(sd, state, &fmt); |
| } |
| |
| dev_dbg(isp_dev->stfcamss->dev, "pad: %d sel(%d,%d)/%dx%d\n", |
| sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height); |
| |
| return 0; |
| } |
| |
| static int isp_init_formats(struct v4l2_subdev *sd, |
| struct v4l2_subdev_state *sd_state) |
| { |
| struct v4l2_subdev_format format = { |
| .pad = STF_ISP_PAD_SINK, |
| .which = V4L2_SUBDEV_FORMAT_ACTIVE, |
| .format = { |
| .code = MEDIA_BUS_FMT_SRGGB10_1X10, |
| .width = 1920, |
| .height = 1080 |
| } |
| }; |
| |
| return isp_set_format(sd, sd_state, &format); |
| } |
| |
| static const struct v4l2_subdev_video_ops isp_video_ops = { |
| .s_stream = isp_set_stream, |
| }; |
| |
| static const struct v4l2_subdev_pad_ops isp_pad_ops = { |
| .enum_mbus_code = isp_enum_mbus_code, |
| .get_fmt = v4l2_subdev_get_fmt, |
| .set_fmt = isp_set_format, |
| .get_selection = isp_get_selection, |
| .set_selection = isp_set_selection, |
| }; |
| |
| static const struct v4l2_subdev_ops isp_v4l2_ops = { |
| .video = &isp_video_ops, |
| .pad = &isp_pad_ops, |
| }; |
| |
| static const struct v4l2_subdev_internal_ops isp_internal_ops = { |
| .init_state = isp_init_formats, |
| }; |
| |
| static const struct media_entity_operations isp_media_ops = { |
| .link_validate = v4l2_subdev_link_validate, |
| }; |
| |
| int stf_isp_register(struct stf_isp_dev *isp_dev, struct v4l2_device *v4l2_dev) |
| { |
| struct v4l2_subdev *sd = &isp_dev->subdev; |
| struct media_pad *pads = isp_dev->pads; |
| int ret; |
| |
| v4l2_subdev_init(sd, &isp_v4l2_ops); |
| sd->internal_ops = &isp_internal_ops; |
| sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; |
| snprintf(sd->name, ARRAY_SIZE(sd->name), "stf_isp"); |
| v4l2_set_subdevdata(sd, isp_dev); |
| |
| pads[STF_ISP_PAD_SINK].flags = MEDIA_PAD_FL_SINK; |
| pads[STF_ISP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE; |
| |
| sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_ISP; |
| sd->entity.ops = &isp_media_ops; |
| ret = media_entity_pads_init(&sd->entity, STF_ISP_PAD_MAX, pads); |
| if (ret) { |
| dev_err(isp_dev->stfcamss->dev, |
| "Failed to init media entity: %d\n", ret); |
| return ret; |
| } |
| |
| ret = v4l2_subdev_init_finalize(sd); |
| if (ret) |
| goto err_entity_cleanup; |
| |
| ret = v4l2_device_register_subdev(v4l2_dev, sd); |
| if (ret) { |
| dev_err(isp_dev->stfcamss->dev, |
| "Failed to register subdev: %d\n", ret); |
| goto err_subdev_cleanup; |
| } |
| |
| return 0; |
| |
| err_subdev_cleanup: |
| v4l2_subdev_cleanup(sd); |
| err_entity_cleanup: |
| media_entity_cleanup(&sd->entity); |
| return ret; |
| } |
| |
| int stf_isp_unregister(struct stf_isp_dev *isp_dev) |
| { |
| v4l2_device_unregister_subdev(&isp_dev->subdev); |
| v4l2_subdev_cleanup(&isp_dev->subdev); |
| media_entity_cleanup(&isp_dev->subdev.entity); |
| |
| return 0; |
| } |