#include <linux/v4l2-mediabus.h>
#include <media/v4l2-rect.h>
#include <media/v4l2-subdev.h>
#include "dcmipp-common.h"
#include "dcmipp-pixelcommon.h"
#define DCMIPP_P1FSCR 0x804
#define DCMIPP_P1FSCR_PIPEDIFF BIT(18)
#define DCMIPP_P1SRCR 0x820
#define DCMIPP_P1SRCR_LASTLINE_SHIFT 0
#define DCMIPP_P1SRCR_FIRSTLINEDEL_SHIFT 12
#define DCMIPP_P1SRCR_CROPEN BIT(15)
#define DCMIPP_P1DECR 0x830
#define DCMIPP_P1DECR_ENABLE BIT(0)
#define DCMIPP_P1DECR_HDEC_SHIFT 1
#define DCMIPP_P1DECR_VDEC_SHIFT 3
#define DCMIPP_P1DMCR 0x870
#define DCMIPP_P1DMCR_ENABLE BIT(0)
#define DCMIPP_P1DMCR_TYPE_SHIFT 1
#define DCMIPP_P1DMCR_TYPE_MASK GENMASK(2, 1)
#define DCMIPP_P1DMCR_TYPE_RGGB 0x0
#define DCMIPP_P1DMCR_TYPE_GRBG 0x1
#define DCMIPP_P1DMCR_TYPE_GBRG 0x2
#define DCMIPP_P1DMCR_TYPE_BGGR 0x3
#define ISP_MEDIA_BUS_SINK_FMT_DEFAULT MEDIA_BUS_FMT_RGB565_1X16
#define ISP_MEDIA_BUS_SRC_FMT_DEFAULT MEDIA_BUS_FMT_RGB888_1X24
struct dcmipp_isp_device {
struct dcmipp_ent_device ved;
struct v4l2_subdev sd;
struct device *dev;
void __iomem *regs;
};
static const struct v4l2_mbus_framefmt fmt_default = {
.width = DCMIPP_FMT_WIDTH_DEFAULT,
.height = DCMIPP_FMT_HEIGHT_DEFAULT,
.code = ISP_MEDIA_BUS_SINK_FMT_DEFAULT,
.field = V4L2_FIELD_NONE,
.colorspace = DCMIPP_COLORSPACE_DEFAULT,
.ycbcr_enc = DCMIPP_YCBCR_ENC_DEFAULT,
.quantization = DCMIPP_QUANTIZATION_DEFAULT,
.xfer_func = DCMIPP_XFER_FUNC_DEFAULT,
};
static inline unsigned int dcmipp_isp_set_compose(__u32 size, __u32 req)
{
unsigned int i = 0;
if (req > size)
return size;
while (size > req && i++ < 3)
size /= 2;
return size;
}
static void dcmipp_isp_adjust_fmt(struct v4l2_mbus_framefmt *fmt, u32 pad)
{
if (!dcmipp_pixelpipe_pix_map_by_code(fmt->code, DCMIPP_ISP, pad))
fmt->code = IS_SRC(pad) ? ISP_MEDIA_BUS_SRC_FMT_DEFAULT :
ISP_MEDIA_BUS_SINK_FMT_DEFAULT;
fmt->width = clamp_t(u32, fmt->width, DCMIPP_FRAME_MIN_WIDTH,
DCMIPP_FRAME_MAX_WIDTH) & ~1;
fmt->height = clamp_t(u32, fmt->height, DCMIPP_FRAME_MIN_HEIGHT,
DCMIPP_FRAME_MAX_HEIGHT);
if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
fmt->field = V4L2_FIELD_NONE;
dcmipp_colorimetry_clamp(fmt);
}
static int dcmipp_isp_init_state(struct v4l2_subdev *sd,
struct v4l2_subdev_state *state)
{
for (unsigned int i = 0; i < sd->entity.num_pads; i++) {
struct v4l2_mbus_framefmt *mf;
mf = v4l2_subdev_state_get_format(state, i);
*mf = fmt_default;
mf->code = IS_SRC(i) ? ISP_MEDIA_BUS_SRC_FMT_DEFAULT :
ISP_MEDIA_BUS_SINK_FMT_DEFAULT;
if (IS_SINK(i)) {
struct v4l2_rect r = {
.top = 0,
.left = 0,
.width = DCMIPP_FMT_WIDTH_DEFAULT,
.height = DCMIPP_FMT_HEIGHT_DEFAULT,
};
*v4l2_subdev_state_get_crop(state, i) = r;
*v4l2_subdev_state_get_compose(state, i) = r;
}
}
return 0;
}
static int dcmipp_isp_enum_mbus_code(struct v4l2_subdev *sd,
struct v4l2_subdev_state *state,
struct v4l2_subdev_mbus_code_enum *code)
{
return dcmipp_pixelpipe_enum_mbus_code(DCMIPP_ISP, code);
}
static int dcmipp_isp_enum_frame_size(struct v4l2_subdev *sd,
struct v4l2_subdev_state *state,
struct v4l2_subdev_frame_size_enum *fse)
{
return dcmipp_pixelpipe_enum_frame_size(DCMIPP_ISP, fse);
}
static int dcmipp_isp_set_fmt(struct v4l2_subdev *sd,
const struct v4l2_subdev_client_info *ci,
struct v4l2_subdev_state *state,
struct v4l2_subdev_format *fmt)
{
if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
v4l2_subdev_is_streaming(sd))
return -EBUSY;
dcmipp_isp_adjust_fmt(&fmt->format, fmt->pad);
if (IS_SINK(fmt->pad)) {
struct v4l2_mbus_framefmt *src_fmt =
v4l2_subdev_state_get_format(state, 1);
struct v4l2_rect r = {
.top = 0,
.left = 0,
.width = fmt->format.width,
.height = fmt->format.height,
};
*v4l2_subdev_state_get_crop(state, 0) = r;
*v4l2_subdev_state_get_compose(state, 0) = r;
*src_fmt = fmt->format;
src_fmt->code = dcmipp_pixelpipe_src_format(fmt->format.code);
*v4l2_subdev_state_get_format(state, 2) = *src_fmt;
} else {
struct v4l2_mbus_framefmt *sink_fmt =
v4l2_subdev_state_get_format(state, 0);
struct v4l2_rect *compose =
v4l2_subdev_state_get_compose(state, 0);
fmt->format = *sink_fmt;
fmt->format.code = dcmipp_pixelpipe_src_format(sink_fmt->code);
if (compose->width && compose->height) {
fmt->format.width = compose->width;
fmt->format.height = compose->height;
}
*v4l2_subdev_state_get_format(state, fmt->pad == 1 ? 2 : 1) =
fmt->format;
}
*v4l2_subdev_state_get_format(state, fmt->pad) = fmt->format;
return 0;
}
static void dcmipp_isp_adjust_crop(struct v4l2_rect *r,
const struct v4l2_mbus_framefmt *fmt)
{
struct v4l2_rect crop_min = {
.width = fmt->width,
.height = DCMIPP_FRAME_MIN_HEIGHT,
};
#define DCMIPP_ISP_MAX_TOP_CROP 7
v4l2_rect_set_min_size(r, &crop_min);
if (r->top > DCMIPP_ISP_MAX_TOP_CROP)
r->top = DCMIPP_ISP_MAX_TOP_CROP;
if ((r->height + r->top) > fmt->height)
r->height = fmt->height - r->top;
}
static int dcmipp_isp_set_selection(struct v4l2_subdev *sd,
const struct v4l2_subdev_client_info *ci,
struct v4l2_subdev_state *state,
struct v4l2_subdev_selection *s)
{
struct dcmipp_isp_device *isp = v4l2_get_subdevdata(sd);
struct v4l2_mbus_framefmt *sink_fmt, *src_fmt;
struct v4l2_rect *crop, *compose;
__u32 pad;
if (IS_SRC(s->pad))
return -EINVAL;
if (s->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
v4l2_subdev_is_streaming(sd))
return -EBUSY;
crop = v4l2_subdev_state_get_crop(state, s->pad);
compose = v4l2_subdev_state_get_compose(state, s->pad);
switch (s->target) {
case V4L2_SEL_TGT_CROP:
sink_fmt = v4l2_subdev_state_get_format(state, s->pad);
dcmipp_isp_adjust_crop(&s->r, sink_fmt);
*crop = s->r;
*compose = s->r;
dev_dbg(isp->dev, "s_selection: crop (%d,%d)/%ux%u\n",
crop->left, crop->top, crop->width, crop->height);
break;
case V4L2_SEL_TGT_COMPOSE:
s->r.top = 0;
s->r.left = 0;
if (s->r.width < DCMIPP_FRAME_MIN_WIDTH)
s->r.width = DCMIPP_FRAME_MIN_WIDTH;
s->r.width = dcmipp_isp_set_compose(crop->width, s->r.width);
if (s->r.height < DCMIPP_FRAME_MIN_HEIGHT)
s->r.height = DCMIPP_FRAME_MIN_HEIGHT;
s->r.height = dcmipp_isp_set_compose(crop->height, s->r.height);
*compose = s->r;
dev_dbg(isp->dev, "s_selection: compose (%d,%d)/%ux%u\n",
compose->left, compose->top,
compose->width, compose->height);
break;
default:
return -EINVAL;
}
for (pad = 1; pad < sd->entity.num_pads; pad++) {
src_fmt = v4l2_subdev_state_get_format(state, pad);
src_fmt->width = s->r.width;
src_fmt->height = s->r.height;
}
return 0;
}
#define STM32_DCMIPP_IS_BAYER_VARIANT(code, variant) \
((code) == MEDIA_BUS_FMT_S##variant##8_1X8 || \
(code) == MEDIA_BUS_FMT_S##variant##10_1X10 || \
(code) == MEDIA_BUS_FMT_S##variant##12_1X12 || \
(code) == MEDIA_BUS_FMT_S##variant##14_1X14 || \
(code) == MEDIA_BUS_FMT_S##variant##16_1X16)
static void dcmipp_isp_config_demosaicing(struct dcmipp_isp_device *isp,
struct v4l2_subdev_state *state)
{
__u32 code = v4l2_subdev_state_get_format(state, 0)->code;
unsigned int val = 0;
reg_clear(isp, DCMIPP_P1DMCR,
DCMIPP_P1DMCR_ENABLE | DCMIPP_P1DMCR_TYPE_MASK);
if (code < MEDIA_BUS_FMT_SBGGR8_1X8 || code >= MEDIA_BUS_FMT_JPEG_1X8)
return;
dev_dbg(isp->dev, "Input is RawBayer, enable Demosaicing\n");
if (STM32_DCMIPP_IS_BAYER_VARIANT(code, BGGR))
val = DCMIPP_P1DMCR_TYPE_BGGR << DCMIPP_P1DMCR_TYPE_SHIFT;
else if (STM32_DCMIPP_IS_BAYER_VARIANT(code, GBRG))
val = DCMIPP_P1DMCR_TYPE_GBRG << DCMIPP_P1DMCR_TYPE_SHIFT;
else if (STM32_DCMIPP_IS_BAYER_VARIANT(code, GRBG))
val = DCMIPP_P1DMCR_TYPE_GRBG << DCMIPP_P1DMCR_TYPE_SHIFT;
else if (STM32_DCMIPP_IS_BAYER_VARIANT(code, RGGB))
val = DCMIPP_P1DMCR_TYPE_RGGB << DCMIPP_P1DMCR_TYPE_SHIFT;
val |= DCMIPP_P1DMCR_ENABLE;
reg_set(isp, DCMIPP_P1DMCR, val);
}
static bool dcmipp_isp_is_aux_output_enabled(struct dcmipp_isp_device *isp)
{
struct media_link *link;
for_each_media_entity_data_link(isp->ved.ent, link) {
if (link->source != &isp->ved.pads[2])
continue;
if (!(link->flags & MEDIA_LNK_FL_ENABLED))
continue;
if (!strcmp(link->sink->entity->name, "dcmipp_aux_postproc"))
return true;
}
return false;
}
static void dcmipp_isp_config_decimation(struct dcmipp_isp_device *isp,
struct v4l2_subdev_state *state)
{
struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
struct v4l2_rect *compose = v4l2_subdev_state_get_compose(state, 0);
u32 decr;
decr = (fls(crop->width / compose->width) - 1) << DCMIPP_P1DECR_HDEC_SHIFT |
(fls(crop->height / compose->height) - 1) << DCMIPP_P1DECR_VDEC_SHIFT;
if (decr)
decr |= DCMIPP_P1DECR_ENABLE;
reg_write(isp, DCMIPP_P1DECR, decr);
}
static int dcmipp_isp_enable_streams(struct v4l2_subdev *sd,
struct v4l2_subdev_state *state,
u32 pad, u64 streams_mask)
{
struct dcmipp_isp_device *isp = v4l2_get_subdevdata(sd);
struct v4l2_rect *crop = v4l2_subdev_state_get_crop(state, 0);
struct v4l2_subdev *s_subdev;
struct media_pad *s_pad;
int ret;
if (sd->enabled_pads)
return 0;
s_pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
if (!s_pad || !is_media_entity_v4l2_subdev(s_pad->entity))
return -EINVAL;
s_subdev = media_entity_to_v4l2_subdev(s_pad->entity);
if (dcmipp_isp_is_aux_output_enabled(isp))
reg_clear(isp, DCMIPP_P1FSCR, DCMIPP_P1FSCR_PIPEDIFF);
else
reg_set(isp, DCMIPP_P1FSCR, DCMIPP_P1FSCR_PIPEDIFF);
crop = v4l2_subdev_state_get_crop(state, 0);
reg_write(isp, DCMIPP_P1SRCR,
((crop->top << DCMIPP_P1SRCR_FIRSTLINEDEL_SHIFT) |
(crop->height << DCMIPP_P1SRCR_LASTLINE_SHIFT) |
DCMIPP_P1SRCR_CROPEN));
dcmipp_isp_config_decimation(isp, state);
dcmipp_isp_config_demosaicing(isp, state);
ret = v4l2_subdev_enable_streams(s_subdev, s_pad->index, BIT_ULL(0));
if (ret < 0) {
dev_err(isp->dev,
"failed to start source subdev streaming (%d)\n", ret);
return ret;
}
return 0;
}
static int dcmipp_isp_disable_streams(struct v4l2_subdev *sd,
struct v4l2_subdev_state *state,
u32 pad, u64 streams_mask)
{
struct dcmipp_isp_device *isp = v4l2_get_subdevdata(sd);
struct v4l2_subdev *s_subdev;
struct media_pad *s_pad;
int ret;
if ((sd->enabled_pads & ~BIT(pad)))
return 0;
s_pad = media_pad_remote_pad_first(&sd->entity.pads[0]);
if (!s_pad || !is_media_entity_v4l2_subdev(s_pad->entity))
return -EINVAL;
s_subdev = media_entity_to_v4l2_subdev(s_pad->entity);
reg_write(isp, DCMIPP_P1SRCR, 0);
reg_write(isp, DCMIPP_P1DECR, 0);
reg_write(isp, DCMIPP_P1DMCR, 0);
ret = v4l2_subdev_disable_streams(s_subdev, s_pad->index, BIT_ULL(0));
if (ret < 0) {
dev_err(isp->dev,
"failed to disable source subdev streaming (%d)\n", ret);
return ret;
}
return 0;
}
static const struct v4l2_subdev_pad_ops dcmipp_isp_pad_ops = {
.enum_mbus_code = dcmipp_isp_enum_mbus_code,
.enum_frame_size = dcmipp_isp_enum_frame_size,
.get_fmt = v4l2_subdev_get_fmt,
.set_fmt = dcmipp_isp_set_fmt,
.get_selection = dcmipp_pixelpipe_get_selection,
.set_selection = dcmipp_isp_set_selection,
.enable_streams = dcmipp_isp_enable_streams,
.disable_streams = dcmipp_isp_disable_streams,
};
static const struct v4l2_subdev_video_ops dcmipp_isp_video_ops = {
.s_stream = v4l2_subdev_s_stream_helper,
};
static const struct v4l2_subdev_ops dcmipp_isp_ops = {
.pad = &dcmipp_isp_pad_ops,
.video = &dcmipp_isp_video_ops,
};
static void dcmipp_isp_release(struct v4l2_subdev *sd)
{
struct dcmipp_isp_device *isp = v4l2_get_subdevdata(sd);
kfree(isp);
}
static const struct v4l2_subdev_internal_ops dcmipp_isp_int_ops = {
.init_state = dcmipp_isp_init_state,
.release = dcmipp_isp_release,
};
void dcmipp_isp_ent_release(struct dcmipp_ent_device *ved)
{
struct dcmipp_isp_device *isp =
container_of(ved, struct dcmipp_isp_device, ved);
dcmipp_ent_sd_unregister(ved, &isp->sd);
}
struct dcmipp_ent_device *dcmipp_isp_ent_init(const char *entity_name,
struct dcmipp_device *dcmipp)
{
struct dcmipp_isp_device *isp;
const unsigned long pads_flag[] = {
MEDIA_PAD_FL_SINK, MEDIA_PAD_FL_SOURCE,
MEDIA_PAD_FL_SOURCE,
};
int ret;
isp = kzalloc_obj(*isp);
if (!isp)
return ERR_PTR(-ENOMEM);
isp->regs = dcmipp->regs;
ret = dcmipp_ent_sd_register(&isp->ved, &isp->sd,
&dcmipp->v4l2_dev, entity_name,
MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER,
ARRAY_SIZE(pads_flag), pads_flag,
&dcmipp_isp_int_ops, &dcmipp_isp_ops,
NULL, NULL);
if (ret) {
kfree(isp);
return ERR_PTR(ret);
}
isp->ved.dcmipp = dcmipp;
isp->dev = dcmipp->dev;
return &isp->ved;
}