#include <media/v4l2-fwnode.h>
#include <media/v4l2-image-sizes.h>
#include <media/v4l2-ioctl.h>
+#include <media/v4l2-rect.h>
#include <media/videobuf2-dma-contig.h>
#define DRV_NAME "stm32-dcmi"
u8 bpp;
};
+struct dcmi_framesize {
+ u32 width;
+ u32 height;
+};
+
struct dcmi_buf {
struct vb2_v4l2_buffer vb;
bool prepared;
struct v4l2_async_notifier notifier;
struct dcmi_graph_entity entity;
struct v4l2_format fmt;
+ struct v4l2_rect crop;
+ bool do_crop;
const struct dcmi_format **sd_formats;
unsigned int num_of_sd_formats;
const struct dcmi_format *sd_format;
+ struct dcmi_framesize *sd_framesizes;
+ unsigned int num_of_sd_framesizes;
+ struct dcmi_framesize sd_framesize;
+ struct v4l2_rect sd_bounds;
/* Protect this data structure */
struct mutex lock;
return 0;
}
+static void dcmi_set_crop(struct stm32_dcmi *dcmi)
+{
+ u32 size, start;
+
+ /* Crop resolution */
+ size = ((dcmi->crop.height - 1) << 16) |
+ ((dcmi->crop.width << 1) - 1);
+ reg_write(dcmi->regs, DCMI_CWSIZE, size);
+
+ /* Crop start point */
+ start = ((dcmi->crop.top) << 16) |
+ ((dcmi->crop.left << 1));
+ reg_write(dcmi->regs, DCMI_CWSTRT, start);
+
+ dev_dbg(dcmi->dev, "Cropping to %ux%u@%u:%u\n",
+ dcmi->crop.width, dcmi->crop.height,
+ dcmi->crop.left, dcmi->crop.top);
+
+ /* Enable crop */
+ reg_set(dcmi->regs, DCMI_CR, CR_CROP);
+}
+
static irqreturn_t dcmi_irq_thread(int irq, void *arg)
{
struct stm32_dcmi *dcmi = arg;
reg_write(dcmi->regs, DCMI_CR, val);
+ /* Set crop */
+ if (dcmi->do_crop)
+ dcmi_set_crop(dcmi);
+
/* Enable dcmi */
reg_set(dcmi->regs, DCMI_CR, CR_ENABLE);
return NULL;
}
+static void __find_outer_frame_size(struct stm32_dcmi *dcmi,
+ struct v4l2_pix_format *pix,
+ struct dcmi_framesize *framesize)
+{
+ struct dcmi_framesize *match = NULL;
+ unsigned int i;
+ unsigned int min_err = UINT_MAX;
+
+ for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
+ struct dcmi_framesize *fsize = &dcmi->sd_framesizes[i];
+ int w_err = (fsize->width - pix->width);
+ int h_err = (fsize->height - pix->height);
+ int err = w_err + h_err;
+
+ if ((w_err >= 0) && (h_err >= 0) && (err < min_err)) {
+ min_err = err;
+ match = fsize;
+ }
+ }
+ if (!match)
+ match = &dcmi->sd_framesizes[0];
+
+ *framesize = *match;
+}
+
static int dcmi_try_fmt(struct stm32_dcmi *dcmi, struct v4l2_format *f,
- const struct dcmi_format **sd_format)
+ const struct dcmi_format **sd_format,
+ struct dcmi_framesize *sd_framesize)
{
const struct dcmi_format *sd_fmt;
+ struct dcmi_framesize sd_fsize;
struct v4l2_pix_format *pix = &f->fmt.pix;
struct v4l2_subdev_pad_config pad_cfg;
struct v4l2_subdev_format format = {
pix->width = clamp(pix->width, MIN_WIDTH, MAX_WIDTH);
pix->height = clamp(pix->height, MIN_HEIGHT, MAX_HEIGHT);
+ if (dcmi->do_crop && dcmi->num_of_sd_framesizes) {
+ struct dcmi_framesize outer_sd_fsize;
+ /*
+ * If crop is requested and sensor have discrete frame sizes,
+ * select the frame size that is just larger than request
+ */
+ __find_outer_frame_size(dcmi, pix, &outer_sd_fsize);
+ pix->width = outer_sd_fsize.width;
+ pix->height = outer_sd_fsize.height;
+ }
+
v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code);
ret = v4l2_subdev_call(dcmi->entity.subdev, pad, set_fmt,
&pad_cfg, &format);
/* Update pix regarding to what sensor can do */
v4l2_fill_pix_format(pix, &format.format);
+ /* Save resolution that sensor can actually do */
+ sd_fsize.width = pix->width;
+ sd_fsize.height = pix->height;
+
+ if (dcmi->do_crop) {
+ struct v4l2_rect c = dcmi->crop;
+ struct v4l2_rect max_rect;
+
+ /*
+ * Adjust crop by making the intersection between
+ * format resolution request and crop request
+ */
+ max_rect.top = 0;
+ max_rect.left = 0;
+ max_rect.width = pix->width;
+ max_rect.height = pix->height;
+ v4l2_rect_map_inside(&c, &max_rect);
+ c.top = clamp_t(s32, c.top, 0, pix->height - c.height);
+ c.left = clamp_t(s32, c.left, 0, pix->width - c.width);
+ dcmi->crop = c;
+
+ /* Adjust format resolution request to crop */
+ pix->width = dcmi->crop.width;
+ pix->height = dcmi->crop.height;
+ }
pix->field = V4L2_FIELD_NONE;
pix->bytesperline = pix->width * sd_fmt->bpp;
if (sd_format)
*sd_format = sd_fmt;
+ if (sd_framesize)
+ *sd_framesize = sd_fsize;
return 0;
}
.which = V4L2_SUBDEV_FORMAT_ACTIVE,
};
const struct dcmi_format *sd_format;
+ struct dcmi_framesize sd_framesize;
struct v4l2_mbus_framefmt *mf = &format.format;
struct v4l2_pix_format *pix = &f->fmt.pix;
int ret;
- ret = dcmi_try_fmt(dcmi, f, &sd_format);
+ /*
+ * Try format, fmt.width/height could have been changed
+ * to match sensor capability or crop request
+ * sd_format & sd_framesize will contain what subdev
+ * can do for this request.
+ */
+ ret = dcmi_try_fmt(dcmi, f, &sd_format, &sd_framesize);
if (ret)
return ret;
/* pix to mbus format */
v4l2_fill_mbus_format(mf, pix,
sd_format->mbus_code);
+ mf->width = sd_framesize.width;
+ mf->height = sd_framesize.height;
+
ret = v4l2_subdev_call(dcmi->entity.subdev, pad,
set_fmt, NULL, &format);
if (ret < 0)
return ret;
+ dev_dbg(dcmi->dev, "Sensor format set to 0x%x %ux%u\n",
+ mf->code, mf->width, mf->height);
+ dev_dbg(dcmi->dev, "Buffer format set to %4.4s %ux%u\n",
+ (char *)&pix->pixelformat,
+ pix->width, pix->height);
+
dcmi->fmt = *f;
dcmi->sd_format = sd_format;
+ dcmi->sd_framesize = sd_framesize;
return 0;
}
{
struct stm32_dcmi *dcmi = video_drvdata(file);
- return dcmi_try_fmt(dcmi, f, NULL);
+ return dcmi_try_fmt(dcmi, f, NULL, NULL);
}
static int dcmi_enum_fmt_vid_cap(struct file *file, void *priv,
return 0;
}
+static int dcmi_get_sensor_format(struct stm32_dcmi *dcmi,
+ struct v4l2_pix_format *pix)
+{
+ struct v4l2_subdev_format fmt = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ };
+ int ret;
+
+ ret = v4l2_subdev_call(dcmi->entity.subdev, pad, get_fmt, NULL, &fmt);
+ if (ret)
+ return ret;
+
+ v4l2_fill_pix_format(pix, &fmt.format);
+
+ return 0;
+}
+
+static int dcmi_set_sensor_format(struct stm32_dcmi *dcmi,
+ struct v4l2_pix_format *pix)
+{
+ const struct dcmi_format *sd_fmt;
+ struct v4l2_subdev_format format = {
+ .which = V4L2_SUBDEV_FORMAT_TRY,
+ };
+ struct v4l2_subdev_pad_config pad_cfg;
+ int ret;
+
+ sd_fmt = find_format_by_fourcc(dcmi, pix->pixelformat);
+ if (!sd_fmt) {
+ sd_fmt = dcmi->sd_formats[dcmi->num_of_sd_formats - 1];
+ pix->pixelformat = sd_fmt->fourcc;
+ }
+
+ v4l2_fill_mbus_format(&format.format, pix, sd_fmt->mbus_code);
+ ret = v4l2_subdev_call(dcmi->entity.subdev, pad, set_fmt,
+ &pad_cfg, &format);
+ if (ret < 0)
+ return ret;
+
+ return 0;
+}
+
+static int dcmi_get_sensor_bounds(struct stm32_dcmi *dcmi,
+ struct v4l2_rect *r)
+{
+ struct v4l2_subdev_selection bounds = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ .target = V4L2_SEL_TGT_CROP_BOUNDS,
+ };
+ unsigned int max_width, max_height, max_pixsize;
+ struct v4l2_pix_format pix;
+ unsigned int i;
+ int ret;
+
+ /*
+ * Get sensor bounds first
+ */
+ ret = v4l2_subdev_call(dcmi->entity.subdev, pad, get_selection,
+ NULL, &bounds);
+ if (!ret)
+ *r = bounds.r;
+ if (ret != -ENOIOCTLCMD)
+ return ret;
+
+ /*
+ * If selection is not implemented,
+ * fallback by enumerating sensor frame sizes
+ * and take the largest one
+ */
+ max_width = 0;
+ max_height = 0;
+ max_pixsize = 0;
+ for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
+ struct dcmi_framesize *fsize = &dcmi->sd_framesizes[i];
+ unsigned int pixsize = fsize->width * fsize->height;
+
+ if (pixsize > max_pixsize) {
+ max_pixsize = pixsize;
+ max_width = fsize->width;
+ max_height = fsize->height;
+ }
+ }
+ if (max_pixsize > 0) {
+ r->top = 0;
+ r->left = 0;
+ r->width = max_width;
+ r->height = max_height;
+ return 0;
+ }
+
+ /*
+ * If frame sizes enumeration is not implemented,
+ * fallback by getting current sensor frame size
+ */
+ ret = dcmi_get_sensor_format(dcmi, &pix);
+ if (ret)
+ return ret;
+
+ r->top = 0;
+ r->left = 0;
+ r->width = pix.width;
+ r->height = pix.height;
+
+ return 0;
+}
+
+static int dcmi_g_selection(struct file *file, void *fh,
+ struct v4l2_selection *s)
+{
+ struct stm32_dcmi *dcmi = video_drvdata(file);
+
+ if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ return -EINVAL;
+
+ switch (s->target) {
+ case V4L2_SEL_TGT_CROP_DEFAULT:
+ case V4L2_SEL_TGT_CROP_BOUNDS:
+ s->r = dcmi->sd_bounds;
+ return 0;
+ case V4L2_SEL_TGT_CROP:
+ if (dcmi->do_crop) {
+ s->r = dcmi->crop;
+ } else {
+ s->r.top = 0;
+ s->r.left = 0;
+ s->r.width = dcmi->fmt.fmt.pix.width;
+ s->r.height = dcmi->fmt.fmt.pix.height;
+ }
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int dcmi_s_selection(struct file *file, void *priv,
+ struct v4l2_selection *s)
+{
+ struct stm32_dcmi *dcmi = video_drvdata(file);
+ struct v4l2_rect r = s->r;
+ struct v4l2_rect max_rect;
+ struct v4l2_pix_format pix;
+
+ if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
+ s->target != V4L2_SEL_TGT_CROP)
+ return -EINVAL;
+
+ /* Reset sensor resolution to max resolution */
+ pix.pixelformat = dcmi->fmt.fmt.pix.pixelformat;
+ pix.width = dcmi->sd_bounds.width;
+ pix.height = dcmi->sd_bounds.height;
+ dcmi_set_sensor_format(dcmi, &pix);
+
+ /*
+ * Make the intersection between
+ * sensor resolution
+ * and crop request
+ */
+ max_rect.top = 0;
+ max_rect.left = 0;
+ max_rect.width = pix.width;
+ max_rect.height = pix.height;
+ v4l2_rect_map_inside(&r, &max_rect);
+ r.top = clamp_t(s32, r.top, 0, pix.height - r.height);
+ r.left = clamp_t(s32, r.left, 0, pix.width - r.width);
+
+ if (!((r.top == dcmi->sd_bounds.top) &&
+ (r.left == dcmi->sd_bounds.left) &&
+ (r.width == dcmi->sd_bounds.width) &&
+ (r.height == dcmi->sd_bounds.height))) {
+ /* Crop if request is different than sensor resolution */
+ dcmi->do_crop = true;
+ dcmi->crop = r;
+ dev_dbg(dcmi->dev, "s_selection: crop %ux%u@(%u,%u) from %ux%u\n",
+ r.width, r.height, r.left, r.top,
+ pix.width, pix.height);
+ } else {
+ /* Disable crop */
+ dcmi->do_crop = false;
+ dev_dbg(dcmi->dev, "s_selection: crop is disabled\n");
+ }
+
+ s->r = r;
+ return 0;
+}
+
static int dcmi_querycap(struct file *file, void *priv,
struct v4l2_capability *cap)
{
.vidioc_g_fmt_vid_cap = dcmi_g_fmt_vid_cap,
.vidioc_s_fmt_vid_cap = dcmi_s_fmt_vid_cap,
.vidioc_enum_fmt_vid_cap = dcmi_enum_fmt_vid_cap,
+ .vidioc_g_selection = dcmi_g_selection,
+ .vidioc_s_selection = dcmi_s_selection,
.vidioc_enum_input = dcmi_enum_input,
.vidioc_g_input = dcmi_g_input,
};
int ret;
- ret = dcmi_try_fmt(dcmi, &f, NULL);
+ ret = dcmi_try_fmt(dcmi, &f, NULL, NULL);
if (ret)
return ret;
dcmi->sd_format = dcmi->sd_formats[0];
return 0;
}
+static int dcmi_framesizes_init(struct stm32_dcmi *dcmi)
+{
+ unsigned int num_fsize = 0;
+ struct v4l2_subdev *subdev = dcmi->entity.subdev;
+ struct v4l2_subdev_frame_size_enum fse = {
+ .which = V4L2_SUBDEV_FORMAT_ACTIVE,
+ .code = dcmi->sd_format->mbus_code,
+ };
+ unsigned int ret;
+ unsigned int i;
+
+ /* Allocate discrete framesizes array */
+ while (!v4l2_subdev_call(subdev, pad, enum_frame_size,
+ NULL, &fse))
+ fse.index++;
+
+ num_fsize = fse.index;
+ if (!num_fsize)
+ return 0;
+
+ dcmi->num_of_sd_framesizes = num_fsize;
+ dcmi->sd_framesizes = devm_kcalloc(dcmi->dev, num_fsize,
+ sizeof(struct dcmi_framesize),
+ GFP_KERNEL);
+ if (!dcmi->sd_framesizes) {
+ dev_err(dcmi->dev, "Could not allocate memory\n");
+ return -ENOMEM;
+ }
+
+ /* Fill array with sensor supported framesizes */
+ dev_dbg(dcmi->dev, "Sensor supports %u frame sizes:\n", num_fsize);
+ for (i = 0; i < dcmi->num_of_sd_framesizes; i++) {
+ fse.index = i;
+ ret = v4l2_subdev_call(subdev, pad, enum_frame_size,
+ NULL, &fse);
+ if (ret)
+ return ret;
+ dcmi->sd_framesizes[fse.index].width = fse.max_width;
+ dcmi->sd_framesizes[fse.index].height = fse.max_height;
+ dev_dbg(dcmi->dev, "%ux%u\n", fse.max_width, fse.max_height);
+ }
+
+ return 0;
+}
+
static int dcmi_graph_notify_complete(struct v4l2_async_notifier *notifier)
{
struct stm32_dcmi *dcmi = notifier_to_dcmi(notifier);
return ret;
}
+ ret = dcmi_framesizes_init(dcmi);
+ if (ret) {
+ dev_err(dcmi->dev, "Could not initialize framesizes\n");
+ return ret;
+ }
+
+ ret = dcmi_get_sensor_bounds(dcmi, &dcmi->sd_bounds);
+ if (ret) {
+ dev_err(dcmi->dev, "Could not get sensor bounds\n");
+ return ret;
+ }
+
ret = dcmi_set_default_fmt(dcmi);
if (ret) {
dev_err(dcmi->dev, "Could not set default format\n");
projects / users / hch / xfs.git / commitdiff