cal-video.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * TI Camera Access Layer (CAL) - Video Device
   4 *
   5 * Copyright (c) 2015-2020 Texas Instruments Inc.
   6 *
   7 * Authors:
   8 *	Benoit Parrot <bparrot@ti.com>
   9 *	Laurent Pinchart <laurent.pinchart@ideasonboard.com>
  10 */
  11
  12#include <linux/ioctl.h>
  13#include <linux/pm_runtime.h>
  14#include <linux/videodev2.h>
  15
  16#include <media/media-device.h>
  17#include <media/v4l2-common.h>
  18#include <media/v4l2-ctrls.h>
  19#include <media/v4l2-device.h>
  20#include <media/v4l2-event.h>
  21#include <media/v4l2-fh.h>
  22#include <media/v4l2-ioctl.h>
  23#include <media/videobuf2-core.h>
  24#include <media/videobuf2-dma-contig.h>
  25
  26#include "cal.h"
  27
  28/* ------------------------------------------------------------------
  29 *	V4L2 Common IOCTLs
  30 * ------------------------------------------------------------------
  31 */
  32
  33static int cal_querycap(struct file *file, void *priv,
  34			struct v4l2_capability *cap)
  35{
  36	strscpy(cap->driver, CAL_MODULE_NAME, sizeof(cap->driver));
  37	strscpy(cap->card, CAL_MODULE_NAME, sizeof(cap->card));
  38
  39	return 0;
  40}
  41
  42static int cal_g_fmt_vid_cap(struct file *file, void *priv,
  43			     struct v4l2_format *f)
  44{
  45	struct cal_ctx *ctx = video_drvdata(file);
  46
  47	*f = ctx->v_fmt;
  48
  49	return 0;
  50}
  51
  52/* ------------------------------------------------------------------
  53 *	V4L2 Video Node Centric IOCTLs
  54 * ------------------------------------------------------------------
  55 */
  56
  57static const struct cal_format_info *find_format_by_pix(struct cal_ctx *ctx,
  58							u32 pixelformat)
  59{
  60	const struct cal_format_info *fmtinfo;
  61	unsigned int k;
  62
  63	for (k = 0; k < ctx->num_active_fmt; k++) {
  64		fmtinfo = ctx->active_fmt[k];
  65		if (fmtinfo->fourcc == pixelformat)
  66			return fmtinfo;
  67	}
  68
  69	return NULL;
  70}
  71
  72static const struct cal_format_info *find_format_by_code(struct cal_ctx *ctx,
  73							 u32 code)
  74{
  75	const struct cal_format_info *fmtinfo;
  76	unsigned int k;
  77
  78	for (k = 0; k < ctx->num_active_fmt; k++) {
  79		fmtinfo = ctx->active_fmt[k];
  80		if (fmtinfo->code == code)
  81			return fmtinfo;
  82	}
  83
  84	return NULL;
  85}
  86
  87static int cal_legacy_enum_fmt_vid_cap(struct file *file, void *priv,
  88				       struct v4l2_fmtdesc *f)
  89{
  90	struct cal_ctx *ctx = video_drvdata(file);
  91	const struct cal_format_info *fmtinfo;
  92
  93	if (f->index >= ctx->num_active_fmt)
  94		return -EINVAL;
  95
  96	fmtinfo = ctx->active_fmt[f->index];
  97
  98	f->pixelformat = fmtinfo->fourcc;
  99	f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 100	return 0;
 101}
 102
 103static int __subdev_get_format(struct cal_ctx *ctx,
 104			       struct v4l2_mbus_framefmt *fmt)
 105{
 106	struct v4l2_subdev_format sd_fmt = {
 107		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 108		.pad = 0,
 109	};
 110	struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
 111	struct v4l2_subdev *sd = ctx->phy->source;
 112	int ret;
 113
 114	ret = v4l2_subdev_call_state_active(sd, pad, get_fmt, &sd_fmt);
 115	if (ret)
 116		return ret;
 117
 118	*fmt = *mbus_fmt;
 119
 120	ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__,
 121		fmt->width, fmt->height, fmt->code);
 122
 123	return 0;
 124}
 125
 126static int __subdev_set_format(struct cal_ctx *ctx,
 127			       struct v4l2_mbus_framefmt *fmt)
 128{
 129	struct v4l2_subdev_format sd_fmt = {
 130		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 131		.pad = 0,
 132	};
 133	struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
 134	struct v4l2_subdev *sd = ctx->phy->source;
 135	int ret;
 136
 137	*mbus_fmt = *fmt;
 138
 139	ret = v4l2_subdev_call_state_active(sd, pad, set_fmt, &sd_fmt);
 140	if (ret)
 141		return ret;
 142
 143	ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__,
 144		fmt->width, fmt->height, fmt->code);
 145
 146	return 0;
 147}
 148
 149static void cal_calc_format_size(struct cal_ctx *ctx,
 150				 const struct cal_format_info *fmtinfo,
 151				 struct v4l2_format *f)
 152{
 153	u32 bpl, max_width;
 154
 155	/*
 156	 * Maximum width is bound by the DMA max width in bytes.
 157	 * We need to recalculate the actual maxi width depending on the
 158	 * number of bytes per pixels required.
 159	 */
 160	max_width = CAL_MAX_WIDTH_BYTES / (ALIGN(fmtinfo->bpp, 8) >> 3);
 161	v4l_bound_align_image(&f->fmt.pix.width, 48, max_width, 2,
 162			      &f->fmt.pix.height, 32, CAL_MAX_HEIGHT_LINES,
 163			      0, 0);
 164
 165	bpl = (f->fmt.pix.width * ALIGN(fmtinfo->bpp, 8)) >> 3;
 166	f->fmt.pix.bytesperline = ALIGN(bpl, 16);
 167
 168	f->fmt.pix.sizeimage = f->fmt.pix.height *
 169			       f->fmt.pix.bytesperline;
 170
 171	ctx_dbg(3, ctx, "%s: fourcc: %p4cc size: %dx%d bpl:%d img_size:%d\n",
 172		__func__, &f->fmt.pix.pixelformat,
 173		f->fmt.pix.width, f->fmt.pix.height,
 174		f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
 175}
 176
 177static int cal_legacy_try_fmt_vid_cap(struct file *file, void *priv,
 178				      struct v4l2_format *f)
 179{
 180	struct cal_ctx *ctx = video_drvdata(file);
 181	struct v4l2_subdev *sd = ctx->phy->source;
 182	const struct cal_format_info *fmtinfo;
 183	struct v4l2_subdev_frame_size_enum fse = {
 184		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 185	};
 186	int found;
 187
 188	fmtinfo = find_format_by_pix(ctx, f->fmt.pix.pixelformat);
 189	if (!fmtinfo) {
 190		ctx_dbg(3, ctx, "Fourcc format (0x%08x) not found.\n",
 191			f->fmt.pix.pixelformat);
 192
 193		/* Just get the first one enumerated */
 194		fmtinfo = ctx->active_fmt[0];
 195		f->fmt.pix.pixelformat = fmtinfo->fourcc;
 196	}
 197
 198	f->fmt.pix.field = ctx->v_fmt.fmt.pix.field;
 199
 200	/* check for/find a valid width/height */
 201	found = false;
 202	fse.pad = 0;
 203	fse.code = fmtinfo->code;
 204	for (fse.index = 0; ; fse.index++) {
 205		int ret;
 206
 207		ret = v4l2_subdev_call_state_active(sd, pad, enum_frame_size,
 208						    &fse);
 209		if (ret)
 210			break;
 211
 212		if ((f->fmt.pix.width == fse.max_width) &&
 213		    (f->fmt.pix.height == fse.max_height)) {
 214			found = true;
 215			break;
 216		} else if ((f->fmt.pix.width >= fse.min_width) &&
 217			 (f->fmt.pix.width <= fse.max_width) &&
 218			 (f->fmt.pix.height >= fse.min_height) &&
 219			 (f->fmt.pix.height <= fse.max_height)) {
 220			found = true;
 221			break;
 222		}
 223	}
 224
 225	if (!found) {
 226		/* use existing values as default */
 227		f->fmt.pix.width = ctx->v_fmt.fmt.pix.width;
 228		f->fmt.pix.height =  ctx->v_fmt.fmt.pix.height;
 229	}
 230
 231	/*
 232	 * Use current colorspace for now, it will get
 233	 * updated properly during s_fmt
 234	 */
 235	f->fmt.pix.colorspace = ctx->v_fmt.fmt.pix.colorspace;
 236	cal_calc_format_size(ctx, fmtinfo, f);
 237	return 0;
 238}
 239
 240static int cal_legacy_s_fmt_vid_cap(struct file *file, void *priv,
 241				    struct v4l2_format *f)
 242{
 243	struct cal_ctx *ctx = video_drvdata(file);
 244	struct v4l2_subdev *sd = &ctx->phy->subdev;
 245	struct vb2_queue *q = &ctx->vb_vidq;
 246	struct v4l2_subdev_format sd_fmt = {
 247		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 248		.pad = CAL_CAMERARX_PAD_SINK,
 249	};
 250	const struct cal_format_info *fmtinfo;
 251	int ret;
 252
 253	if (vb2_is_busy(q)) {
 254		ctx_dbg(3, ctx, "%s device busy\n", __func__);
 255		return -EBUSY;
 256	}
 257
 258	ret = cal_legacy_try_fmt_vid_cap(file, priv, f);
 259	if (ret < 0)
 260		return ret;
 261
 262	fmtinfo = find_format_by_pix(ctx, f->fmt.pix.pixelformat);
 263
 264	v4l2_fill_mbus_format(&sd_fmt.format, &f->fmt.pix, fmtinfo->code);
 265
 266	ret = __subdev_set_format(ctx, &sd_fmt.format);
 267	if (ret)
 268		return ret;
 269
 270	/* Just double check nothing has gone wrong */
 271	if (sd_fmt.format.code != fmtinfo->code) {
 272		ctx_dbg(3, ctx,
 273			"%s subdev changed format on us, this should not happen\n",
 274			__func__);
 275		return -EINVAL;
 276	}
 277
 278	v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &sd_fmt.format);
 279	ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 280	ctx->v_fmt.fmt.pix.pixelformat = fmtinfo->fourcc;
 281	ctx->v_fmt.fmt.pix.field = sd_fmt.format.field;
 282	cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt);
 283
 284	v4l2_subdev_call_state_active(sd, pad, set_fmt, &sd_fmt);
 285
 286	ctx->fmtinfo = fmtinfo;
 287	*f = ctx->v_fmt;
 288
 289	return 0;
 290}
 291
 292static int cal_legacy_enum_framesizes(struct file *file, void *fh,
 293				      struct v4l2_frmsizeenum *fsize)
 294{
 295	struct cal_ctx *ctx = video_drvdata(file);
 296	struct v4l2_subdev *sd = ctx->phy->source;
 297	const struct cal_format_info *fmtinfo;
 298	struct v4l2_subdev_frame_size_enum fse = {
 299		.index = fsize->index,
 300		.pad = 0,
 301		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 302	};
 303	int ret;
 304
 305	/* check for valid format */
 306	fmtinfo = find_format_by_pix(ctx, fsize->pixel_format);
 307	if (!fmtinfo) {
 308		ctx_dbg(3, ctx, "Invalid pixel code: %x\n",
 309			fsize->pixel_format);
 310		return -EINVAL;
 311	}
 312
 313	fse.code = fmtinfo->code;
 314
 315	ret = v4l2_subdev_call_state_active(sd, pad, enum_frame_size, &fse);
 316	if (ret)
 317		return ret;
 318
 319	ctx_dbg(1, ctx, "%s: index: %d code: %x W:[%d,%d] H:[%d,%d]\n",
 320		__func__, fse.index, fse.code, fse.min_width, fse.max_width,
 321		fse.min_height, fse.max_height);
 322
 323	fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 324	fsize->discrete.width = fse.max_width;
 325	fsize->discrete.height = fse.max_height;
 326
 327	return 0;
 328}
 329
 330static int cal_legacy_enum_input(struct file *file, void *priv,
 331				 struct v4l2_input *inp)
 332{
 333	if (inp->index > 0)
 334		return -EINVAL;
 335
 336	inp->type = V4L2_INPUT_TYPE_CAMERA;
 337	sprintf(inp->name, "Camera %u", inp->index);
 338	return 0;
 339}
 340
 341static int cal_legacy_g_input(struct file *file, void *priv, unsigned int *i)
 342{
 343	*i = 0;
 344	return 0;
 345}
 346
 347static int cal_legacy_s_input(struct file *file, void *priv, unsigned int i)
 348{
 349	return i > 0 ? -EINVAL : 0;
 350}
 351
 352/* timeperframe is arbitrary and continuous */
 353static int cal_legacy_enum_frameintervals(struct file *file, void *priv,
 354					  struct v4l2_frmivalenum *fival)
 355{
 356	struct cal_ctx *ctx = video_drvdata(file);
 357	struct v4l2_subdev *sd = ctx->phy->source;
 358	const struct cal_format_info *fmtinfo;
 359	struct v4l2_subdev_frame_interval_enum fie = {
 360		.index = fival->index,
 361		.width = fival->width,
 362		.height = fival->height,
 363		.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 364	};
 365	int ret;
 366
 367	fmtinfo = find_format_by_pix(ctx, fival->pixel_format);
 368	if (!fmtinfo)
 369		return -EINVAL;
 370
 371	fie.code = fmtinfo->code;
 372
 373	ret = v4l2_subdev_call_state_active(sd, pad, enum_frame_interval, &fie);
 374	if (ret)
 375		return ret;
 376	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
 377	fival->discrete = fie.interval;
 378
 379	return 0;
 380}
 381
 382static int cal_legacy_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
 383{
 384	struct cal_ctx *ctx = video_drvdata(file);
 385
 386	return v4l2_g_parm_cap(video_devdata(file), ctx->phy->source, a);
 387}
 388
 389static int cal_legacy_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
 390{
 391	struct cal_ctx *ctx = video_drvdata(file);
 392
 393	return v4l2_s_parm_cap(video_devdata(file), ctx->phy->source, a);
 394}
 395
 396static const struct v4l2_ioctl_ops cal_ioctl_legacy_ops = {
 397	.vidioc_querycap      = cal_querycap,
 398	.vidioc_enum_fmt_vid_cap  = cal_legacy_enum_fmt_vid_cap,
 399	.vidioc_g_fmt_vid_cap     = cal_g_fmt_vid_cap,
 400	.vidioc_try_fmt_vid_cap   = cal_legacy_try_fmt_vid_cap,
 401	.vidioc_s_fmt_vid_cap     = cal_legacy_s_fmt_vid_cap,
 402	.vidioc_enum_framesizes   = cal_legacy_enum_framesizes,
 403	.vidioc_reqbufs       = vb2_ioctl_reqbufs,
 404	.vidioc_create_bufs   = vb2_ioctl_create_bufs,
 405	.vidioc_prepare_buf   = vb2_ioctl_prepare_buf,
 406	.vidioc_querybuf      = vb2_ioctl_querybuf,
 407	.vidioc_qbuf          = vb2_ioctl_qbuf,
 408	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
 409	.vidioc_expbuf        = vb2_ioctl_expbuf,
 410	.vidioc_enum_input    = cal_legacy_enum_input,
 411	.vidioc_g_input       = cal_legacy_g_input,
 412	.vidioc_s_input       = cal_legacy_s_input,
 413	.vidioc_enum_frameintervals = cal_legacy_enum_frameintervals,
 414	.vidioc_streamon      = vb2_ioctl_streamon,
 415	.vidioc_streamoff     = vb2_ioctl_streamoff,
 416	.vidioc_log_status    = v4l2_ctrl_log_status,
 417	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
 418	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 419	.vidioc_g_parm		= cal_legacy_g_parm,
 420	.vidioc_s_parm		= cal_legacy_s_parm,
 421};
 422
 423/* ------------------------------------------------------------------
 424 *	V4L2 Media Controller Centric IOCTLs
 425 * ------------------------------------------------------------------
 426 */
 427
 428static int cal_mc_enum_fmt_vid_cap(struct file *file, void  *priv,
 429				   struct v4l2_fmtdesc *f)
 430{
 431	unsigned int i;
 432	unsigned int idx;
 433
 434	if (f->index >= cal_num_formats)
 435		return -EINVAL;
 436
 437	idx = 0;
 438
 439	for (i = 0; i < cal_num_formats; ++i) {
 440		if (cal_formats[i].meta)
 441			continue;
 442
 443		if (f->mbus_code && cal_formats[i].code != f->mbus_code)
 444			continue;
 445
 446		if (idx == f->index) {
 447			f->pixelformat = cal_formats[i].fourcc;
 448			f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 449			return 0;
 450		}
 451
 452		idx++;
 453	}
 454
 455	return -EINVAL;
 456}
 457
 458static void cal_mc_try_fmt(struct cal_ctx *ctx, struct v4l2_format *f,
 459			   const struct cal_format_info **info)
 460{
 461	struct v4l2_pix_format *format = &f->fmt.pix;
 462	const struct cal_format_info *fmtinfo;
 463	unsigned int bpp;
 464
 465	/*
 466	 * Default to the first format if the requested pixel format code isn't
 467	 * supported.
 468	 */
 469	fmtinfo = cal_format_by_fourcc(f->fmt.pix.pixelformat);
 470	if (!fmtinfo || fmtinfo->meta)
 471		fmtinfo = &cal_formats[0];
 472
 473	/*
 474	 * Clamp the size, update the pixel format. The field and colorspace are
 475	 * accepted as-is, except for V4L2_FIELD_ANY that is turned into
 476	 * V4L2_FIELD_NONE.
 477	 */
 478	bpp = ALIGN(fmtinfo->bpp, 8);
 479
 480	format->width = clamp_t(unsigned int, format->width,
 481				CAL_MIN_WIDTH_BYTES * 8 / bpp,
 482				CAL_MAX_WIDTH_BYTES * 8 / bpp);
 483	format->height = clamp_t(unsigned int, format->height,
 484				 CAL_MIN_HEIGHT_LINES, CAL_MAX_HEIGHT_LINES);
 485	format->pixelformat = fmtinfo->fourcc;
 486
 487	if (format->field == V4L2_FIELD_ANY)
 488		format->field = V4L2_FIELD_NONE;
 489
 490	/*
 491	 * Calculate the number of bytes per line and the image size. The
 492	 * hardware stores the stride as a number of 16 bytes words, in a
 493	 * signed 15-bit value. Only 14 bits are thus usable.
 494	 */
 495	format->bytesperline = ALIGN(clamp(format->bytesperline,
 496					   format->width * bpp / 8,
 497					   ((1U << 14) - 1) * 16), 16);
 498
 499	format->sizeimage = format->height * format->bytesperline;
 500
 501	format->colorspace = ctx->v_fmt.fmt.pix.colorspace;
 502
 503	if (info)
 504		*info = fmtinfo;
 505
 506	ctx_dbg(3, ctx, "%s: %p4cc %ux%u (bytesperline %u sizeimage %u)\n",
 507		__func__, &format->pixelformat,
 508		format->width, format->height,
 509		format->bytesperline, format->sizeimage);
 510}
 511
 512static int cal_mc_try_fmt_vid_cap(struct file *file, void *priv,
 513				  struct v4l2_format *f)
 514{
 515	struct cal_ctx *ctx = video_drvdata(file);
 516
 517	cal_mc_try_fmt(ctx, f, NULL);
 518	return 0;
 519}
 520
 521static int cal_mc_s_fmt_vid_cap(struct file *file, void *priv,
 522				struct v4l2_format *f)
 523{
 524	struct cal_ctx *ctx = video_drvdata(file);
 525	const struct cal_format_info *fmtinfo;
 526
 527	if (vb2_is_busy(&ctx->vb_vidq)) {
 528		ctx_dbg(3, ctx, "%s device busy\n", __func__);
 529		return -EBUSY;
 530	}
 531
 532	cal_mc_try_fmt(ctx, f, &fmtinfo);
 533
 534	ctx->v_fmt = *f;
 535	ctx->fmtinfo = fmtinfo;
 536
 537	return 0;
 538}
 539
 540static int cal_mc_enum_framesizes(struct file *file, void *fh,
 541				  struct v4l2_frmsizeenum *fsize)
 542{
 543	struct cal_ctx *ctx = video_drvdata(file);
 544	const struct cal_format_info *fmtinfo;
 545	unsigned int bpp;
 546
 547	if (fsize->index > 0)
 548		return -EINVAL;
 549
 550	fmtinfo = cal_format_by_fourcc(fsize->pixel_format);
 551	if (!fmtinfo) {
 552		ctx_dbg(3, ctx, "Invalid pixel format 0x%08x\n",
 553			fsize->pixel_format);
 554		return -EINVAL;
 555	}
 556
 557	bpp = ALIGN(fmtinfo->bpp, 8);
 558
 559	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
 560	fsize->stepwise.min_width = CAL_MIN_WIDTH_BYTES * 8 / bpp;
 561	fsize->stepwise.max_width = CAL_MAX_WIDTH_BYTES * 8 / bpp;
 562	fsize->stepwise.step_width = 64 / bpp;
 563	fsize->stepwise.min_height = CAL_MIN_HEIGHT_LINES;
 564	fsize->stepwise.max_height = CAL_MAX_HEIGHT_LINES;
 565	fsize->stepwise.step_height = 1;
 566
 567	return 0;
 568}
 569
 570static const struct v4l2_ioctl_ops cal_ioctl_mc_ops = {
 571	.vidioc_querycap      = cal_querycap,
 572	.vidioc_enum_fmt_vid_cap  = cal_mc_enum_fmt_vid_cap,
 573	.vidioc_g_fmt_vid_cap     = cal_g_fmt_vid_cap,
 574	.vidioc_try_fmt_vid_cap   = cal_mc_try_fmt_vid_cap,
 575	.vidioc_s_fmt_vid_cap     = cal_mc_s_fmt_vid_cap,
 576	.vidioc_enum_framesizes   = cal_mc_enum_framesizes,
 577	.vidioc_reqbufs       = vb2_ioctl_reqbufs,
 578	.vidioc_create_bufs   = vb2_ioctl_create_bufs,
 579	.vidioc_prepare_buf   = vb2_ioctl_prepare_buf,
 580	.vidioc_querybuf      = vb2_ioctl_querybuf,
 581	.vidioc_qbuf          = vb2_ioctl_qbuf,
 582	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
 583	.vidioc_expbuf        = vb2_ioctl_expbuf,
 584	.vidioc_streamon      = vb2_ioctl_streamon,
 585	.vidioc_streamoff     = vb2_ioctl_streamoff,
 586	.vidioc_log_status    = v4l2_ctrl_log_status,
 587};
 588
 589/* ------------------------------------------------------------------
 590 *	videobuf2 Common Operations
 591 * ------------------------------------------------------------------
 592 */
 593
 594static int cal_queue_setup(struct vb2_queue *vq,
 595			   unsigned int *nbuffers, unsigned int *nplanes,
 596			   unsigned int sizes[], struct device *alloc_devs[])
 597{
 598	struct cal_ctx *ctx = vb2_get_drv_priv(vq);
 599	unsigned int size = ctx->v_fmt.fmt.pix.sizeimage;
 600	unsigned int q_num_bufs = vb2_get_num_buffers(vq);
 601
 602	if (q_num_bufs + *nbuffers < 3)
 603		*nbuffers = 3 - q_num_bufs;
 604
 605	if (*nplanes) {
 606		if (sizes[0] < size)
 607			return -EINVAL;
 608		size = sizes[0];
 609	}
 610
 611	*nplanes = 1;
 612	sizes[0] = size;
 613
 614	ctx_dbg(3, ctx, "nbuffers=%d, size=%d\n", *nbuffers, sizes[0]);
 615
 616	return 0;
 617}
 618
 619static int cal_buffer_prepare(struct vb2_buffer *vb)
 620{
 621	struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 622	struct cal_buffer *buf = container_of(vb, struct cal_buffer,
 623					      vb.vb2_buf);
 624	unsigned long size;
 625
 626	size = ctx->v_fmt.fmt.pix.sizeimage;
 627	if (vb2_plane_size(vb, 0) < size) {
 628		ctx_err(ctx,
 629			"data will not fit into plane (%lu < %lu)\n",
 630			vb2_plane_size(vb, 0), size);
 631		return -EINVAL;
 632	}
 633
 634	vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);
 635	return 0;
 636}
 637
 638static void cal_buffer_queue(struct vb2_buffer *vb)
 639{
 640	struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 641	struct cal_buffer *buf = container_of(vb, struct cal_buffer,
 642					      vb.vb2_buf);
 643	unsigned long flags;
 644
 645	/* recheck locking */
 646	spin_lock_irqsave(&ctx->dma.lock, flags);
 647	list_add_tail(&buf->list, &ctx->dma.queue);
 648	spin_unlock_irqrestore(&ctx->dma.lock, flags);
 649}
 650
 651static void cal_release_buffers(struct cal_ctx *ctx,
 652				enum vb2_buffer_state state)
 653{
 654	struct cal_buffer *buf, *tmp;
 655
 656	/* Release all queued buffers. */
 657	spin_lock_irq(&ctx->dma.lock);
 658
 659	list_for_each_entry_safe(buf, tmp, &ctx->dma.queue, list) {
 660		list_del(&buf->list);
 661		vb2_buffer_done(&buf->vb.vb2_buf, state);
 662	}
 663
 664	if (ctx->dma.pending) {
 665		vb2_buffer_done(&ctx->dma.pending->vb.vb2_buf, state);
 666		ctx->dma.pending = NULL;
 667	}
 668
 669	if (ctx->dma.active) {
 670		vb2_buffer_done(&ctx->dma.active->vb.vb2_buf, state);
 671		ctx->dma.active = NULL;
 672	}
 673
 674	spin_unlock_irq(&ctx->dma.lock);
 675}
 676
 677/* ------------------------------------------------------------------
 678 *	videobuf2 Operations
 679 * ------------------------------------------------------------------
 680 */
 681
 682static int cal_video_check_format(struct cal_ctx *ctx)
 683{
 684	const struct v4l2_mbus_framefmt *format;
 685	struct v4l2_subdev_state *state;
 686	struct media_pad *phy_source_pad;
 687	int ret = 0;
 688
 689	phy_source_pad = media_pad_remote_pad_first(&ctx->pad);
 690	if (!phy_source_pad)
 691		return -ENODEV;
 692
 693	state = v4l2_subdev_lock_and_get_active_state(&ctx->phy->subdev);
 694
 695	format = v4l2_subdev_state_get_format(state, phy_source_pad->index, 0);
 696	if (!format) {
 697		ret = -EINVAL;
 698		goto out;
 699	}
 700
 701	if (ctx->fmtinfo->code != format->code ||
 702	    ctx->v_fmt.fmt.pix.height != format->height ||
 703	    ctx->v_fmt.fmt.pix.width != format->width ||
 704	    ctx->v_fmt.fmt.pix.field != format->field) {
 705		ret = -EPIPE;
 706		goto out;
 707	}
 708
 709out:
 710	v4l2_subdev_unlock_state(state);
 711
 712	return ret;
 713}
 714
 715static int cal_start_streaming(struct vb2_queue *vq, unsigned int count)
 716{
 717	struct cal_ctx *ctx = vb2_get_drv_priv(vq);
 718	struct media_pad *phy_source_pad;
 719	struct cal_buffer *buf;
 720	dma_addr_t addr;
 721	int ret;
 722
 723	phy_source_pad = media_pad_remote_pad_first(&ctx->pad);
 724	if (!phy_source_pad) {
 725		ctx_err(ctx, "Context not connected\n");
 726		ret = -ENODEV;
 727		goto error_release_buffers;
 728	}
 729
 730	ret = video_device_pipeline_alloc_start(&ctx->vdev);
 731	if (ret < 0) {
 732		ctx_err(ctx, "Failed to start media pipeline: %d\n", ret);
 733		goto error_release_buffers;
 734	}
 735
 736	/* Find the PHY connected to this video device */
 737	if (cal_mc_api)
 738		ctx->phy = cal_camerarx_get_phy_from_entity(phy_source_pad->entity);
 739
 740	/*
 741	 * Verify that the currently configured format matches the output of
 742	 * the connected CAMERARX.
 743	 */
 744	ret = cal_video_check_format(ctx);
 745	if (ret < 0) {
 746		ctx_dbg(3, ctx,
 747			"Format mismatch between CAMERARX and video node\n");
 748		goto error_pipeline;
 749	}
 750
 751	ret = cal_ctx_prepare(ctx);
 752	if (ret) {
 753		ctx_err(ctx, "Failed to prepare context: %d\n", ret);
 754		goto error_pipeline;
 755	}
 756
 757	spin_lock_irq(&ctx->dma.lock);
 758	buf = list_first_entry(&ctx->dma.queue, struct cal_buffer, list);
 759	ctx->dma.active = buf;
 760	list_del(&buf->list);
 761	spin_unlock_irq(&ctx->dma.lock);
 762
 763	addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
 764
 765	ret = pm_runtime_resume_and_get(ctx->cal->dev);
 766	if (ret < 0)
 767		goto error_unprepare;
 768
 769	cal_ctx_set_dma_addr(ctx, addr);
 770	cal_ctx_start(ctx);
 771
 772	ret = v4l2_subdev_enable_streams(&ctx->phy->subdev,
 773					 phy_source_pad->index, BIT(0));
 774	if (ret)
 775		goto error_stop;
 776
 777	if (cal_debug >= 4)
 778		cal_quickdump_regs(ctx->cal);
 779
 780	return 0;
 781
 782error_stop:
 783	cal_ctx_stop(ctx);
 784	pm_runtime_put_sync(ctx->cal->dev);
 785error_unprepare:
 786	cal_ctx_unprepare(ctx);
 787error_pipeline:
 788	video_device_pipeline_stop(&ctx->vdev);
 789error_release_buffers:
 790	cal_release_buffers(ctx, VB2_BUF_STATE_QUEUED);
 791
 792	return ret;
 793}
 794
 795static void cal_stop_streaming(struct vb2_queue *vq)
 796{
 797	struct cal_ctx *ctx = vb2_get_drv_priv(vq);
 798	struct media_pad *phy_source_pad;
 799
 800	cal_ctx_stop(ctx);
 801
 802	phy_source_pad = media_pad_remote_pad_first(&ctx->pad);
 803
 804	v4l2_subdev_disable_streams(&ctx->phy->subdev, phy_source_pad->index,
 805				    BIT(0));
 806
 807	pm_runtime_put_sync(ctx->cal->dev);
 808
 809	cal_ctx_unprepare(ctx);
 810
 811	cal_release_buffers(ctx, VB2_BUF_STATE_ERROR);
 812
 813	video_device_pipeline_stop(&ctx->vdev);
 814
 815	if (cal_mc_api)
 816		ctx->phy = NULL;
 817}
 818
 819static const struct vb2_ops cal_video_qops = {
 820	.queue_setup		= cal_queue_setup,
 821	.buf_prepare		= cal_buffer_prepare,
 822	.buf_queue		= cal_buffer_queue,
 823	.start_streaming	= cal_start_streaming,
 824	.stop_streaming		= cal_stop_streaming,
 825};
 826
 827/* ------------------------------------------------------------------
 828 *	V4L2 Initialization and Registration
 829 * ------------------------------------------------------------------
 830 */
 831
 832static const struct v4l2_file_operations cal_fops = {
 833	.owner		= THIS_MODULE,
 834	.open           = v4l2_fh_open,
 835	.release        = vb2_fop_release,
 836	.poll		= vb2_fop_poll,
 837	.unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
 838	.mmap           = vb2_fop_mmap,
 839};
 840
 841static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
 842{
 843	struct v4l2_subdev *sd = ctx->phy->source;
 844	struct v4l2_mbus_framefmt mbus_fmt;
 845	const struct cal_format_info *fmtinfo;
 846	unsigned int i, j, k;
 847	int ret = 0;
 848
 849	/* Enumerate sub device formats and enable all matching local formats */
 850	ctx->active_fmt = devm_kcalloc(ctx->cal->dev, cal_num_formats,
 851				       sizeof(*ctx->active_fmt), GFP_KERNEL);
 852	if (!ctx->active_fmt)
 853		return -ENOMEM;
 854
 855	ctx->num_active_fmt = 0;
 856
 857	for (j = 0, i = 0; ; ++j) {
 858		struct v4l2_subdev_mbus_code_enum mbus_code = {
 859			.index = j,
 860			.which = V4L2_SUBDEV_FORMAT_ACTIVE,
 861		};
 862
 863		ret = v4l2_subdev_call_state_active(sd, pad, enum_mbus_code,
 864						    &mbus_code);
 865		if (ret == -EINVAL)
 866			break;
 867
 868		if (ret) {
 869			ctx_err(ctx, "Error enumerating mbus codes in subdev %s: %d\n",
 870				sd->name, ret);
 871			return ret;
 872		}
 873
 874		ctx_dbg(2, ctx,
 875			"subdev %s: code: %04x idx: %u\n",
 876			sd->name, mbus_code.code, j);
 877
 878		for (k = 0; k < cal_num_formats; k++) {
 879			fmtinfo = &cal_formats[k];
 880
 881			if (mbus_code.code == fmtinfo->code) {
 882				ctx->active_fmt[i] = fmtinfo;
 883				ctx_dbg(2, ctx,
 884					"matched fourcc: %p4cc: code: %04x idx: %u\n",
 885					&fmtinfo->fourcc,
 886					fmtinfo->code, i);
 887				ctx->num_active_fmt = ++i;
 888			}
 889		}
 890	}
 891
 892	if (i == 0) {
 893		ctx_err(ctx, "No suitable format reported by subdev %s\n",
 894			sd->name);
 895		return -EINVAL;
 896	}
 897
 898	ret = __subdev_get_format(ctx, &mbus_fmt);
 899	if (ret)
 900		return ret;
 901
 902	fmtinfo = find_format_by_code(ctx, mbus_fmt.code);
 903	if (!fmtinfo) {
 904		ctx_dbg(3, ctx, "mbus code format (0x%08x) not found.\n",
 905			mbus_fmt.code);
 906		return -EINVAL;
 907	}
 908
 909	/* Save current format */
 910	v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt);
 911	ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 912	ctx->v_fmt.fmt.pix.pixelformat = fmtinfo->fourcc;
 913	cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt);
 914	ctx->fmtinfo = fmtinfo;
 915
 916	return 0;
 917}
 918
 919static int cal_ctx_v4l2_init_mc_format(struct cal_ctx *ctx)
 920{
 921	const struct cal_format_info *fmtinfo;
 922	struct v4l2_pix_format *pix_fmt = &ctx->v_fmt.fmt.pix;
 923
 924	fmtinfo = cal_format_by_code(MEDIA_BUS_FMT_UYVY8_1X16);
 925	if (!fmtinfo)
 926		return -EINVAL;
 927
 928	pix_fmt->width = 640;
 929	pix_fmt->height = 480;
 930	pix_fmt->field = V4L2_FIELD_NONE;
 931	pix_fmt->colorspace = V4L2_COLORSPACE_SRGB;
 932	pix_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
 933	pix_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
 934	pix_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
 935	pix_fmt->pixelformat = fmtinfo->fourcc;
 936
 937	ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 938
 939	/* Save current format */
 940	cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt);
 941	ctx->fmtinfo = fmtinfo;
 942
 943	return 0;
 944}
 945
 946int cal_ctx_v4l2_register(struct cal_ctx *ctx)
 947{
 948	struct video_device *vfd = &ctx->vdev;
 949	int ret;
 950
 951	if (!cal_mc_api) {
 952		struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler;
 953
 954		ret = cal_ctx_v4l2_init_formats(ctx);
 955		if (ret) {
 956			ctx_err(ctx, "Failed to init formats: %d\n", ret);
 957			return ret;
 958		}
 959
 960		ret = v4l2_ctrl_add_handler(hdl, ctx->phy->source->ctrl_handler,
 961					    NULL, true);
 962		if (ret < 0) {
 963			ctx_err(ctx, "Failed to add source ctrl handler\n");
 964			return ret;
 965		}
 966	} else {
 967		ret = cal_ctx_v4l2_init_mc_format(ctx);
 968		if (ret) {
 969			ctx_err(ctx, "Failed to init format: %d\n", ret);
 970			return ret;
 971		}
 972	}
 973
 974	ret = video_register_device(vfd, VFL_TYPE_VIDEO, cal_video_nr);
 975	if (ret < 0) {
 976		ctx_err(ctx, "Failed to register video device\n");
 977		return ret;
 978	}
 979
 980	if (cal_mc_api) {
 981		u16 phy_idx;
 982		u16 pad_idx;
 983
 984		/* Create links from all video nodes to all PHYs */
 985
 986		for (phy_idx = 0; phy_idx < ctx->cal->data->num_csi2_phy;
 987		     ++phy_idx) {
 988			struct media_entity *phy_entity =
 989				&ctx->cal->phy[phy_idx]->subdev.entity;
 990
 991			for (pad_idx = 1; pad_idx < CAL_CAMERARX_NUM_PADS;
 992			     ++pad_idx) {
 993				/*
 994				 * Enable only links from video0 to PHY0 pad 1,
 995				 * and video1 to PHY1 pad 1.
 996				 */
 997				bool enable = (ctx->dma_ctx == 0 &&
 998					       phy_idx == 0 && pad_idx == 1) ||
 999					      (ctx->dma_ctx == 1 &&
1000					       phy_idx == 1 && pad_idx == 1);
1001
1002				ret = media_create_pad_link(phy_entity, pad_idx,
1003							    &vfd->entity, 0,
1004							    enable ? MEDIA_LNK_FL_ENABLED : 0);
1005				if (ret) {
1006					ctx_err(ctx,
1007						"Failed to create media link for context %u\n",
1008						ctx->dma_ctx);
1009					video_unregister_device(vfd);
1010					return ret;
1011				}
1012			}
1013		}
1014	} else {
1015		ret = media_create_pad_link(&ctx->phy->subdev.entity,
1016					    CAL_CAMERARX_PAD_FIRST_SOURCE,
1017					    &vfd->entity, 0,
1018					    MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1019		if (ret) {
1020			ctx_err(ctx,
1021				"Failed to create media link for context %u\n",
1022				ctx->dma_ctx);
1023			video_unregister_device(vfd);
1024			return ret;
1025		}
1026	}
1027
1028	ctx_info(ctx, "V4L2 device registered as %s\n",
1029		 video_device_node_name(vfd));
1030
1031	return 0;
1032}
1033
1034void cal_ctx_v4l2_unregister(struct cal_ctx *ctx)
1035{
1036	ctx_dbg(1, ctx, "unregistering %s\n",
1037		video_device_node_name(&ctx->vdev));
1038
1039	video_unregister_device(&ctx->vdev);
1040}
1041
1042int cal_ctx_v4l2_init(struct cal_ctx *ctx)
1043{
1044	struct video_device *vfd = &ctx->vdev;
1045	struct vb2_queue *q = &ctx->vb_vidq;
1046	int ret;
1047
1048	INIT_LIST_HEAD(&ctx->dma.queue);
1049	spin_lock_init(&ctx->dma.lock);
1050	mutex_init(&ctx->mutex);
1051	init_waitqueue_head(&ctx->dma.wait);
1052
1053	/* Initialize the vb2 queue. */
1054	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1055	q->io_modes = VB2_MMAP | VB2_DMABUF;
1056	q->drv_priv = ctx;
1057	q->buf_struct_size = sizeof(struct cal_buffer);
1058	q->ops = &cal_video_qops;
1059	q->mem_ops = &vb2_dma_contig_memops;
1060	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1061	q->lock = &ctx->mutex;
1062	q->min_queued_buffers = 3;
1063	q->dev = ctx->cal->dev;
1064
1065	ret = vb2_queue_init(q);
1066	if (ret)
1067		return ret;
1068
1069	/* Initialize the video device and media entity. */
1070	vfd->fops = &cal_fops;
1071	vfd->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
1072			 | (cal_mc_api ? V4L2_CAP_IO_MC : 0);
1073	vfd->v4l2_dev = &ctx->cal->v4l2_dev;
1074	vfd->queue = q;
1075	snprintf(vfd->name, sizeof(vfd->name), "CAL output %u", ctx->dma_ctx);
1076	vfd->release = video_device_release_empty;
1077	vfd->ioctl_ops = cal_mc_api ? &cal_ioctl_mc_ops : &cal_ioctl_legacy_ops;
1078	vfd->lock = &ctx->mutex;
1079	video_set_drvdata(vfd, ctx);
1080
1081	ctx->pad.flags = MEDIA_PAD_FL_SINK;
1082	ret = media_entity_pads_init(&vfd->entity, 1, &ctx->pad);
1083	if (ret < 0)
1084		return ret;
1085
1086	if (!cal_mc_api) {
1087		/* Initialize the control handler. */
1088		struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler;
1089
1090		ret = v4l2_ctrl_handler_init(hdl, 11);
1091		if (ret < 0) {
1092			ctx_err(ctx, "Failed to init ctrl handler\n");
1093			goto error;
1094		}
1095
1096		vfd->ctrl_handler = hdl;
1097	}
1098
1099	return 0;
1100
1101error:
1102	media_entity_cleanup(&vfd->entity);
1103	return ret;
1104}
1105
1106void cal_ctx_v4l2_cleanup(struct cal_ctx *ctx)
1107{
1108	if (!cal_mc_api)
1109		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
1110
1111	media_entity_cleanup(&ctx->vdev.entity);
1112}