1/* 2 * V4L2 Driver for i.MX27 camera host 3 * 4 * Copyright (C) 2008, Sascha Hauer, Pengutronix 5 * Copyright (C) 2010, Baruch Siach, Orex Computed Radiography 6 * Copyright (C) 2012, Javier Martin, Vista Silicon S.L. 7 * 8 * This program is free software; you can redistribute it and/or modify 9 * it under the terms of the GNU General Public License as published by 10 * the Free Software Foundation; either version 2 of the License, or 11 * (at your option) any later version. 12 */ 13 14#include <linux/init.h> 15#include <linux/module.h> 16#include <linux/io.h> 17#include <linux/delay.h> 18#include <linux/slab.h> 19#include <linux/dma-mapping.h> 20#include <linux/errno.h> 21#include <linux/fs.h> 22#include <linux/gcd.h> 23#include <linux/interrupt.h> 24#include <linux/kernel.h> 25#include <linux/math64.h> 26#include <linux/mm.h> 27#include <linux/moduleparam.h> 28#include <linux/time.h> 29#include <linux/device.h> 30#include <linux/platform_device.h> 31#include <linux/clk.h> 32 33#include <media/v4l2-common.h> 34#include <media/v4l2-dev.h> 35#include <media/videobuf2-v4l2.h> 36#include <media/videobuf2-dma-contig.h> 37#include <media/soc_camera.h> 38#include <media/soc_mediabus.h> 39 40#include <linux/videodev2.h> 41 42#include <linux/platform_data/camera-mx2.h> 43 44#include <asm/dma.h> 45 46#define MX2_CAM_DRV_NAME "mx2-camera" 47#define MX2_CAM_VERSION "0.0.6" 48#define MX2_CAM_DRIVER_DESCRIPTION "i.MX2x_Camera" 49 50/* reset values */ 51#define CSICR1_RESET_VAL 0x40000800 52#define CSICR2_RESET_VAL 0x0 53#define CSICR3_RESET_VAL 0x0 54 55/* csi control reg 1 */ 56#define CSICR1_SWAP16_EN (1 << 31) 57#define CSICR1_EXT_VSYNC (1 << 30) 58#define CSICR1_EOF_INTEN (1 << 29) 59#define CSICR1_PRP_IF_EN (1 << 28) 60#define CSICR1_CCIR_MODE (1 << 27) 61#define CSICR1_COF_INTEN (1 << 26) 62#define CSICR1_SF_OR_INTEN (1 << 25) 63#define CSICR1_RF_OR_INTEN (1 << 24) 64#define CSICR1_STATFF_LEVEL (3 << 22) 65#define CSICR1_STATFF_INTEN (1 << 21) 66#define CSICR1_RXFF_LEVEL(l) (((l) & 3) << 19) 67#define CSICR1_RXFF_INTEN (1 << 18) 68#define CSICR1_SOF_POL (1 << 17) 69#define CSICR1_SOF_INTEN (1 << 16) 70#define CSICR1_MCLKDIV(d) (((d) & 0xF) << 12) 71#define CSICR1_HSYNC_POL (1 << 11) 72#define CSICR1_CCIR_EN (1 << 10) 73#define CSICR1_MCLKEN (1 << 9) 74#define CSICR1_FCC (1 << 8) 75#define CSICR1_PACK_DIR (1 << 7) 76#define CSICR1_CLR_STATFIFO (1 << 6) 77#define CSICR1_CLR_RXFIFO (1 << 5) 78#define CSICR1_GCLK_MODE (1 << 4) 79#define CSICR1_INV_DATA (1 << 3) 80#define CSICR1_INV_PCLK (1 << 2) 81#define CSICR1_REDGE (1 << 1) 82#define CSICR1_FMT_MASK (CSICR1_PACK_DIR | CSICR1_SWAP16_EN) 83 84#define SHIFT_STATFF_LEVEL 22 85#define SHIFT_RXFF_LEVEL 19 86#define SHIFT_MCLKDIV 12 87 88#define SHIFT_FRMCNT 16 89 90#define CSICR1 0x00 91#define CSICR2 0x04 92#define CSISR 0x08 93#define CSISTATFIFO 0x0c 94#define CSIRFIFO 0x10 95#define CSIRXCNT 0x14 96#define CSICR3 0x1c 97#define CSIDMASA_STATFIFO 0x20 98#define CSIDMATA_STATFIFO 0x24 99#define CSIDMASA_FB1 0x28 100#define CSIDMASA_FB2 0x2c 101#define CSIFBUF_PARA 0x30 102#define CSIIMAG_PARA 0x34 103 104/* EMMA PrP */ 105#define PRP_CNTL 0x00 106#define PRP_INTR_CNTL 0x04 107#define PRP_INTRSTATUS 0x08 108#define PRP_SOURCE_Y_PTR 0x0c 109#define PRP_SOURCE_CB_PTR 0x10 110#define PRP_SOURCE_CR_PTR 0x14 111#define PRP_DEST_RGB1_PTR 0x18 112#define PRP_DEST_RGB2_PTR 0x1c 113#define PRP_DEST_Y_PTR 0x20 114#define PRP_DEST_CB_PTR 0x24 115#define PRP_DEST_CR_PTR 0x28 116#define PRP_SRC_FRAME_SIZE 0x2c 117#define PRP_DEST_CH1_LINE_STRIDE 0x30 118#define PRP_SRC_PIXEL_FORMAT_CNTL 0x34 119#define PRP_CH1_PIXEL_FORMAT_CNTL 0x38 120#define PRP_CH1_OUT_IMAGE_SIZE 0x3c 121#define PRP_CH2_OUT_IMAGE_SIZE 0x40 122#define PRP_SRC_LINE_STRIDE 0x44 123#define PRP_CSC_COEF_012 0x48 124#define PRP_CSC_COEF_345 0x4c 125#define PRP_CSC_COEF_678 0x50 126#define PRP_CH1_RZ_HORI_COEF1 0x54 127#define PRP_CH1_RZ_HORI_COEF2 0x58 128#define PRP_CH1_RZ_HORI_VALID 0x5c 129#define PRP_CH1_RZ_VERT_COEF1 0x60 130#define PRP_CH1_RZ_VERT_COEF2 0x64 131#define PRP_CH1_RZ_VERT_VALID 0x68 132#define PRP_CH2_RZ_HORI_COEF1 0x6c 133#define PRP_CH2_RZ_HORI_COEF2 0x70 134#define PRP_CH2_RZ_HORI_VALID 0x74 135#define PRP_CH2_RZ_VERT_COEF1 0x78 136#define PRP_CH2_RZ_VERT_COEF2 0x7c 137#define PRP_CH2_RZ_VERT_VALID 0x80 138 139#define PRP_CNTL_CH1EN (1 << 0) 140#define PRP_CNTL_CH2EN (1 << 1) 141#define PRP_CNTL_CSIEN (1 << 2) 142#define PRP_CNTL_DATA_IN_YUV420 (0 << 3) 143#define PRP_CNTL_DATA_IN_YUV422 (1 << 3) 144#define PRP_CNTL_DATA_IN_RGB16 (2 << 3) 145#define PRP_CNTL_DATA_IN_RGB32 (3 << 3) 146#define PRP_CNTL_CH1_OUT_RGB8 (0 << 5) 147#define PRP_CNTL_CH1_OUT_RGB16 (1 << 5) 148#define PRP_CNTL_CH1_OUT_RGB32 (2 << 5) 149#define PRP_CNTL_CH1_OUT_YUV422 (3 << 5) 150#define PRP_CNTL_CH2_OUT_YUV420 (0 << 7) 151#define PRP_CNTL_CH2_OUT_YUV422 (1 << 7) 152#define PRP_CNTL_CH2_OUT_YUV444 (2 << 7) 153#define PRP_CNTL_CH1_LEN (1 << 9) 154#define PRP_CNTL_CH2_LEN (1 << 10) 155#define PRP_CNTL_SKIP_FRAME (1 << 11) 156#define PRP_CNTL_SWRST (1 << 12) 157#define PRP_CNTL_CLKEN (1 << 13) 158#define PRP_CNTL_WEN (1 << 14) 159#define PRP_CNTL_CH1BYP (1 << 15) 160#define PRP_CNTL_IN_TSKIP(x) ((x) << 16) 161#define PRP_CNTL_CH1_TSKIP(x) ((x) << 19) 162#define PRP_CNTL_CH2_TSKIP(x) ((x) << 22) 163#define PRP_CNTL_INPUT_FIFO_LEVEL(x) ((x) << 25) 164#define PRP_CNTL_RZ_FIFO_LEVEL(x) ((x) << 27) 165#define PRP_CNTL_CH2B1EN (1 << 29) 166#define PRP_CNTL_CH2B2EN (1 << 30) 167#define PRP_CNTL_CH2FEN (1 << 31) 168 169/* IRQ Enable and status register */ 170#define PRP_INTR_RDERR (1 << 0) 171#define PRP_INTR_CH1WERR (1 << 1) 172#define PRP_INTR_CH2WERR (1 << 2) 173#define PRP_INTR_CH1FC (1 << 3) 174#define PRP_INTR_CH2FC (1 << 5) 175#define PRP_INTR_LBOVF (1 << 7) 176#define PRP_INTR_CH2OVF (1 << 8) 177 178/* Resizing registers */ 179#define PRP_RZ_VALID_TBL_LEN(x) ((x) << 24) 180#define PRP_RZ_VALID_BILINEAR (1 << 31) 181 182#define MAX_VIDEO_MEM 16 183 184#define RESIZE_NUM_MIN 1 185#define RESIZE_NUM_MAX 20 186#define BC_COEF 3 187#define SZ_COEF (1 << BC_COEF) 188 189#define RESIZE_DIR_H 0 190#define RESIZE_DIR_V 1 191 192#define RESIZE_ALGO_BILINEAR 0 193#define RESIZE_ALGO_AVERAGING 1 194 195struct mx2_prp_cfg { 196 int channel; 197 u32 in_fmt; 198 u32 out_fmt; 199 u32 src_pixel; 200 u32 ch1_pixel; 201 u32 irq_flags; 202 u32 csicr1; 203}; 204 205/* prp resizing parameters */ 206struct emma_prp_resize { 207 int algo; /* type of algorithm used */ 208 int len; /* number of coefficients */ 209 unsigned char s[RESIZE_NUM_MAX]; /* table of coefficients */ 210}; 211 212/* prp configuration for a client-host fmt pair */ 213struct mx2_fmt_cfg { 214 u32 in_fmt; 215 u32 out_fmt; 216 struct mx2_prp_cfg cfg; 217}; 218 219struct mx2_buf_internal { 220 struct list_head queue; 221 int bufnum; 222 bool discard; 223}; 224 225/* buffer for one video frame */ 226struct mx2_buffer { 227 /* common v4l buffer stuff -- must be first */ 228 struct vb2_v4l2_buffer vb; 229 struct mx2_buf_internal internal; 230}; 231 232enum mx2_camera_type { 233 IMX27_CAMERA, 234}; 235 236struct mx2_camera_dev { 237 struct device *dev; 238 struct soc_camera_host soc_host; 239 struct clk *clk_emma_ahb, *clk_emma_ipg; 240 struct clk *clk_csi_ahb, *clk_csi_per; 241 242 void __iomem *base_csi, *base_emma; 243 244 struct mx2_camera_platform_data *pdata; 245 unsigned long platform_flags; 246 247 struct list_head capture; 248 struct list_head active_bufs; 249 struct list_head discard; 250 251 spinlock_t lock; 252 253 int dma; 254 struct mx2_buffer *active; 255 struct mx2_buffer *fb1_active; 256 struct mx2_buffer *fb2_active; 257 258 u32 csicr1; 259 enum mx2_camera_type devtype; 260 261 struct mx2_buf_internal buf_discard[2]; 262 void *discard_buffer; 263 dma_addr_t discard_buffer_dma; 264 size_t discard_size; 265 struct mx2_fmt_cfg *emma_prp; 266 struct emma_prp_resize resizing[2]; 267 unsigned int s_width, s_height; 268 u32 frame_count; 269 struct vb2_alloc_ctx *alloc_ctx; 270}; 271 272static struct platform_device_id mx2_camera_devtype[] = { 273 { 274 .name = "imx27-camera", 275 .driver_data = IMX27_CAMERA, 276 }, { 277 /* sentinel */ 278 } 279}; 280MODULE_DEVICE_TABLE(platform, mx2_camera_devtype); 281 282static struct mx2_buffer *mx2_ibuf_to_buf(struct mx2_buf_internal *int_buf) 283{ 284 return container_of(int_buf, struct mx2_buffer, internal); 285} 286 287static struct mx2_fmt_cfg mx27_emma_prp_table[] = { 288 /* 289 * This is a generic configuration which is valid for most 290 * prp input-output format combinations. 291 * We set the incoming and outgoing pixelformat to a 292 * 16 Bit wide format and adjust the bytesperline 293 * accordingly. With this configuration the inputdata 294 * will not be changed by the emma and could be any type 295 * of 16 Bit Pixelformat. 296 */ 297 { 298 .in_fmt = 0, 299 .out_fmt = 0, 300 .cfg = { 301 .channel = 1, 302 .in_fmt = PRP_CNTL_DATA_IN_RGB16, 303 .out_fmt = PRP_CNTL_CH1_OUT_RGB16, 304 .src_pixel = 0x2ca00565, /* RGB565 */ 305 .ch1_pixel = 0x2ca00565, /* RGB565 */ 306 .irq_flags = PRP_INTR_RDERR | PRP_INTR_CH1WERR | 307 PRP_INTR_CH1FC | PRP_INTR_LBOVF, 308 .csicr1 = 0, 309 } 310 }, 311 { 312 .in_fmt = MEDIA_BUS_FMT_UYVY8_2X8, 313 .out_fmt = V4L2_PIX_FMT_YUYV, 314 .cfg = { 315 .channel = 1, 316 .in_fmt = PRP_CNTL_DATA_IN_YUV422, 317 .out_fmt = PRP_CNTL_CH1_OUT_YUV422, 318 .src_pixel = 0x22000888, /* YUV422 (YUYV) */ 319 .ch1_pixel = 0x62000888, /* YUV422 (YUYV) */ 320 .irq_flags = PRP_INTR_RDERR | PRP_INTR_CH1WERR | 321 PRP_INTR_CH1FC | PRP_INTR_LBOVF, 322 .csicr1 = CSICR1_SWAP16_EN, 323 } 324 }, 325 { 326 .in_fmt = MEDIA_BUS_FMT_YUYV8_2X8, 327 .out_fmt = V4L2_PIX_FMT_YUYV, 328 .cfg = { 329 .channel = 1, 330 .in_fmt = PRP_CNTL_DATA_IN_YUV422, 331 .out_fmt = PRP_CNTL_CH1_OUT_YUV422, 332 .src_pixel = 0x22000888, /* YUV422 (YUYV) */ 333 .ch1_pixel = 0x62000888, /* YUV422 (YUYV) */ 334 .irq_flags = PRP_INTR_RDERR | PRP_INTR_CH1WERR | 335 PRP_INTR_CH1FC | PRP_INTR_LBOVF, 336 .csicr1 = CSICR1_PACK_DIR, 337 } 338 }, 339 { 340 .in_fmt = MEDIA_BUS_FMT_YUYV8_2X8, 341 .out_fmt = V4L2_PIX_FMT_YUV420, 342 .cfg = { 343 .channel = 2, 344 .in_fmt = PRP_CNTL_DATA_IN_YUV422, 345 .out_fmt = PRP_CNTL_CH2_OUT_YUV420, 346 .src_pixel = 0x22000888, /* YUV422 (YUYV) */ 347 .irq_flags = PRP_INTR_RDERR | PRP_INTR_CH2WERR | 348 PRP_INTR_CH2FC | PRP_INTR_LBOVF | 349 PRP_INTR_CH2OVF, 350 .csicr1 = CSICR1_PACK_DIR, 351 } 352 }, 353 { 354 .in_fmt = MEDIA_BUS_FMT_UYVY8_2X8, 355 .out_fmt = V4L2_PIX_FMT_YUV420, 356 .cfg = { 357 .channel = 2, 358 .in_fmt = PRP_CNTL_DATA_IN_YUV422, 359 .out_fmt = PRP_CNTL_CH2_OUT_YUV420, 360 .src_pixel = 0x22000888, /* YUV422 (YUYV) */ 361 .irq_flags = PRP_INTR_RDERR | PRP_INTR_CH2WERR | 362 PRP_INTR_CH2FC | PRP_INTR_LBOVF | 363 PRP_INTR_CH2OVF, 364 .csicr1 = CSICR1_SWAP16_EN, 365 } 366 }, 367}; 368 369static struct mx2_fmt_cfg *mx27_emma_prp_get_format(u32 in_fmt, u32 out_fmt) 370{ 371 int i; 372 373 for (i = 1; i < ARRAY_SIZE(mx27_emma_prp_table); i++) 374 if ((mx27_emma_prp_table[i].in_fmt == in_fmt) && 375 (mx27_emma_prp_table[i].out_fmt == out_fmt)) { 376 return &mx27_emma_prp_table[i]; 377 } 378 /* If no match return the most generic configuration */ 379 return &mx27_emma_prp_table[0]; 380}; 381 382static void mx27_update_emma_buf(struct mx2_camera_dev *pcdev, 383 unsigned long phys, int bufnum) 384{ 385 struct mx2_fmt_cfg *prp = pcdev->emma_prp; 386 387 if (prp->cfg.channel == 1) { 388 writel(phys, pcdev->base_emma + 389 PRP_DEST_RGB1_PTR + 4 * bufnum); 390 } else { 391 writel(phys, pcdev->base_emma + 392 PRP_DEST_Y_PTR - 0x14 * bufnum); 393 if (prp->out_fmt == V4L2_PIX_FMT_YUV420) { 394 u32 imgsize = pcdev->soc_host.icd->user_height * 395 pcdev->soc_host.icd->user_width; 396 397 writel(phys + imgsize, pcdev->base_emma + 398 PRP_DEST_CB_PTR - 0x14 * bufnum); 399 writel(phys + ((5 * imgsize) / 4), pcdev->base_emma + 400 PRP_DEST_CR_PTR - 0x14 * bufnum); 401 } 402 } 403} 404 405static void mx2_camera_deactivate(struct mx2_camera_dev *pcdev) 406{ 407 clk_disable_unprepare(pcdev->clk_csi_ahb); 408 clk_disable_unprepare(pcdev->clk_csi_per); 409 writel(0, pcdev->base_csi + CSICR1); 410 writel(0, pcdev->base_emma + PRP_CNTL); 411} 412 413static int mx2_camera_add_device(struct soc_camera_device *icd) 414{ 415 dev_info(icd->parent, "Camera driver attached to camera %d\n", 416 icd->devnum); 417 418 return 0; 419} 420 421static void mx2_camera_remove_device(struct soc_camera_device *icd) 422{ 423 dev_info(icd->parent, "Camera driver detached from camera %d\n", 424 icd->devnum); 425} 426 427/* 428 * The following two functions absolutely depend on the fact, that 429 * there can be only one camera on mx2 camera sensor interface 430 */ 431static int mx2_camera_clock_start(struct soc_camera_host *ici) 432{ 433 struct mx2_camera_dev *pcdev = ici->priv; 434 int ret; 435 u32 csicr1; 436 437 ret = clk_prepare_enable(pcdev->clk_csi_ahb); 438 if (ret < 0) 439 return ret; 440 441 ret = clk_prepare_enable(pcdev->clk_csi_per); 442 if (ret < 0) 443 goto exit_csi_ahb; 444 445 csicr1 = CSICR1_MCLKEN | CSICR1_PRP_IF_EN | CSICR1_FCC | 446 CSICR1_RXFF_LEVEL(0); 447 448 pcdev->csicr1 = csicr1; 449 writel(pcdev->csicr1, pcdev->base_csi + CSICR1); 450 451 pcdev->frame_count = 0; 452 453 return 0; 454 455exit_csi_ahb: 456 clk_disable_unprepare(pcdev->clk_csi_ahb); 457 458 return ret; 459} 460 461static void mx2_camera_clock_stop(struct soc_camera_host *ici) 462{ 463 struct mx2_camera_dev *pcdev = ici->priv; 464 465 mx2_camera_deactivate(pcdev); 466} 467 468/* 469 * Videobuf operations 470 */ 471static int mx2_videobuf_setup(struct vb2_queue *vq, 472 const void *parg, 473 unsigned int *count, unsigned int *num_planes, 474 unsigned int sizes[], void *alloc_ctxs[]) 475{ 476 const struct v4l2_format *fmt = parg; 477 struct soc_camera_device *icd = soc_camera_from_vb2q(vq); 478 struct soc_camera_host *ici = to_soc_camera_host(icd->parent); 479 struct mx2_camera_dev *pcdev = ici->priv; 480 481 dev_dbg(icd->parent, "count=%d, size=%d\n", *count, sizes[0]); 482 483 /* TODO: support for VIDIOC_CREATE_BUFS not ready */ 484 if (fmt != NULL) 485 return -ENOTTY; 486 487 alloc_ctxs[0] = pcdev->alloc_ctx; 488 489 sizes[0] = icd->sizeimage; 490 491 if (0 == *count) 492 *count = 32; 493 if (!*num_planes && 494 sizes[0] * *count > MAX_VIDEO_MEM * 1024 * 1024) 495 *count = (MAX_VIDEO_MEM * 1024 * 1024) / sizes[0]; 496 497 *num_planes = 1; 498 499 return 0; 500} 501 502static int mx2_videobuf_prepare(struct vb2_buffer *vb) 503{ 504 struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue); 505 int ret = 0; 506 507 dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__, 508 vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0)); 509 510#ifdef DEBUG 511 /* 512 * This can be useful if you want to see if we actually fill 513 * the buffer with something 514 */ 515 memset((void *)vb2_plane_vaddr(vb, 0), 516 0xaa, vb2_get_plane_payload(vb, 0)); 517#endif 518 519 vb2_set_plane_payload(vb, 0, icd->sizeimage); 520 if (vb2_plane_vaddr(vb, 0) && 521 vb2_get_plane_payload(vb, 0) > vb2_plane_size(vb, 0)) { 522 ret = -EINVAL; 523 goto out; 524 } 525 526 return 0; 527 528out: 529 return ret; 530} 531 532static void mx2_videobuf_queue(struct vb2_buffer *vb) 533{ 534 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 535 struct soc_camera_device *icd = soc_camera_from_vb2q(vb->vb2_queue); 536 struct soc_camera_host *ici = 537 to_soc_camera_host(icd->parent); 538 struct mx2_camera_dev *pcdev = ici->priv; 539 struct mx2_buffer *buf = container_of(vbuf, struct mx2_buffer, vb); 540 unsigned long flags; 541 542 dev_dbg(icd->parent, "%s (vb=0x%p) 0x%p %lu\n", __func__, 543 vb, vb2_plane_vaddr(vb, 0), vb2_get_plane_payload(vb, 0)); 544 545 spin_lock_irqsave(&pcdev->lock, flags); 546 547 list_add_tail(&buf->internal.queue, &pcdev->capture); 548 549 spin_unlock_irqrestore(&pcdev->lock, flags); 550} 551 552static void mx27_camera_emma_buf_init(struct soc_camera_device *icd, 553 int bytesperline) 554{ 555 struct soc_camera_host *ici = 556 to_soc_camera_host(icd->parent); 557 struct mx2_camera_dev *pcdev = ici->priv; 558 struct mx2_fmt_cfg *prp = pcdev->emma_prp; 559 560 writel((pcdev->s_width << 16) | pcdev->s_height, 561 pcdev->base_emma + PRP_SRC_FRAME_SIZE); 562 writel(prp->cfg.src_pixel, 563 pcdev->base_emma + PRP_SRC_PIXEL_FORMAT_CNTL); 564 if (prp->cfg.channel == 1) { 565 writel((icd->user_width << 16) | icd->user_height, 566 pcdev->base_emma + PRP_CH1_OUT_IMAGE_SIZE); 567 writel(bytesperline, 568 pcdev->base_emma + PRP_DEST_CH1_LINE_STRIDE); 569 writel(prp->cfg.ch1_pixel, 570 pcdev->base_emma + PRP_CH1_PIXEL_FORMAT_CNTL); 571 } else { /* channel 2 */ 572 writel((icd->user_width << 16) | icd->user_height, 573 pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE); 574 } 575 576 /* Enable interrupts */ 577 writel(prp->cfg.irq_flags, pcdev->base_emma + PRP_INTR_CNTL); 578} 579 580static void mx2_prp_resize_commit(struct mx2_camera_dev *pcdev) 581{ 582 int dir; 583 584 for (dir = RESIZE_DIR_H; dir <= RESIZE_DIR_V; dir++) { 585 unsigned char *s = pcdev->resizing[dir].s; 586 int len = pcdev->resizing[dir].len; 587 unsigned int coeff[2] = {0, 0}; 588 unsigned int valid = 0; 589 int i; 590 591 if (len == 0) 592 continue; 593 594 for (i = RESIZE_NUM_MAX - 1; i >= 0; i--) { 595 int j; 596 597 j = i > 9 ? 1 : 0; 598 coeff[j] = (coeff[j] << BC_COEF) | 599 (s[i] & (SZ_COEF - 1)); 600 601 if (i == 5 || i == 15) 602 coeff[j] <<= 1; 603 604 valid = (valid << 1) | (s[i] >> BC_COEF); 605 } 606 607 valid |= PRP_RZ_VALID_TBL_LEN(len); 608 609 if (pcdev->resizing[dir].algo == RESIZE_ALGO_BILINEAR) 610 valid |= PRP_RZ_VALID_BILINEAR; 611 612 if (pcdev->emma_prp->cfg.channel == 1) { 613 if (dir == RESIZE_DIR_H) { 614 writel(coeff[0], pcdev->base_emma + 615 PRP_CH1_RZ_HORI_COEF1); 616 writel(coeff[1], pcdev->base_emma + 617 PRP_CH1_RZ_HORI_COEF2); 618 writel(valid, pcdev->base_emma + 619 PRP_CH1_RZ_HORI_VALID); 620 } else { 621 writel(coeff[0], pcdev->base_emma + 622 PRP_CH1_RZ_VERT_COEF1); 623 writel(coeff[1], pcdev->base_emma + 624 PRP_CH1_RZ_VERT_COEF2); 625 writel(valid, pcdev->base_emma + 626 PRP_CH1_RZ_VERT_VALID); 627 } 628 } else { 629 if (dir == RESIZE_DIR_H) { 630 writel(coeff[0], pcdev->base_emma + 631 PRP_CH2_RZ_HORI_COEF1); 632 writel(coeff[1], pcdev->base_emma + 633 PRP_CH2_RZ_HORI_COEF2); 634 writel(valid, pcdev->base_emma + 635 PRP_CH2_RZ_HORI_VALID); 636 } else { 637 writel(coeff[0], pcdev->base_emma + 638 PRP_CH2_RZ_VERT_COEF1); 639 writel(coeff[1], pcdev->base_emma + 640 PRP_CH2_RZ_VERT_COEF2); 641 writel(valid, pcdev->base_emma + 642 PRP_CH2_RZ_VERT_VALID); 643 } 644 } 645 } 646} 647 648static int mx2_start_streaming(struct vb2_queue *q, unsigned int count) 649{ 650 struct soc_camera_device *icd = soc_camera_from_vb2q(q); 651 struct soc_camera_host *ici = 652 to_soc_camera_host(icd->parent); 653 struct mx2_camera_dev *pcdev = ici->priv; 654 struct mx2_fmt_cfg *prp = pcdev->emma_prp; 655 struct vb2_buffer *vb; 656 struct mx2_buffer *buf; 657 unsigned long phys; 658 int bytesperline; 659 unsigned long flags; 660 661 if (count < 2) 662 return -ENOBUFS; 663 664 spin_lock_irqsave(&pcdev->lock, flags); 665 666 buf = list_first_entry(&pcdev->capture, struct mx2_buffer, 667 internal.queue); 668 buf->internal.bufnum = 0; 669 vb = &buf->vb.vb2_buf; 670 671 phys = vb2_dma_contig_plane_dma_addr(vb, 0); 672 mx27_update_emma_buf(pcdev, phys, buf->internal.bufnum); 673 list_move_tail(pcdev->capture.next, &pcdev->active_bufs); 674 675 buf = list_first_entry(&pcdev->capture, struct mx2_buffer, 676 internal.queue); 677 buf->internal.bufnum = 1; 678 vb = &buf->vb.vb2_buf; 679 680 phys = vb2_dma_contig_plane_dma_addr(vb, 0); 681 mx27_update_emma_buf(pcdev, phys, buf->internal.bufnum); 682 list_move_tail(pcdev->capture.next, &pcdev->active_bufs); 683 684 bytesperline = soc_mbus_bytes_per_line(icd->user_width, 685 icd->current_fmt->host_fmt); 686 if (bytesperline < 0) { 687 spin_unlock_irqrestore(&pcdev->lock, flags); 688 return bytesperline; 689 } 690 691 /* 692 * I didn't manage to properly enable/disable the prp 693 * on a per frame basis during running transfers, 694 * thus we allocate a buffer here and use it to 695 * discard frames when no buffer is available. 696 * Feel free to work on this ;) 697 */ 698 pcdev->discard_size = icd->user_height * bytesperline; 699 pcdev->discard_buffer = dma_alloc_coherent(ici->v4l2_dev.dev, 700 pcdev->discard_size, 701 &pcdev->discard_buffer_dma, GFP_ATOMIC); 702 if (!pcdev->discard_buffer) { 703 spin_unlock_irqrestore(&pcdev->lock, flags); 704 return -ENOMEM; 705 } 706 707 pcdev->buf_discard[0].discard = true; 708 list_add_tail(&pcdev->buf_discard[0].queue, 709 &pcdev->discard); 710 711 pcdev->buf_discard[1].discard = true; 712 list_add_tail(&pcdev->buf_discard[1].queue, 713 &pcdev->discard); 714 715 mx2_prp_resize_commit(pcdev); 716 717 mx27_camera_emma_buf_init(icd, bytesperline); 718 719 if (prp->cfg.channel == 1) { 720 writel(PRP_CNTL_CH1EN | 721 PRP_CNTL_CSIEN | 722 prp->cfg.in_fmt | 723 prp->cfg.out_fmt | 724 PRP_CNTL_CH1_LEN | 725 PRP_CNTL_CH1BYP | 726 PRP_CNTL_CH1_TSKIP(0) | 727 PRP_CNTL_IN_TSKIP(0), 728 pcdev->base_emma + PRP_CNTL); 729 } else { 730 writel(PRP_CNTL_CH2EN | 731 PRP_CNTL_CSIEN | 732 prp->cfg.in_fmt | 733 prp->cfg.out_fmt | 734 PRP_CNTL_CH2_LEN | 735 PRP_CNTL_CH2_TSKIP(0) | 736 PRP_CNTL_IN_TSKIP(0), 737 pcdev->base_emma + PRP_CNTL); 738 } 739 spin_unlock_irqrestore(&pcdev->lock, flags); 740 741 return 0; 742} 743 744static void mx2_stop_streaming(struct vb2_queue *q) 745{ 746 struct soc_camera_device *icd = soc_camera_from_vb2q(q); 747 struct soc_camera_host *ici = 748 to_soc_camera_host(icd->parent); 749 struct mx2_camera_dev *pcdev = ici->priv; 750 struct mx2_fmt_cfg *prp = pcdev->emma_prp; 751 unsigned long flags; 752 void *b; 753 u32 cntl; 754 755 spin_lock_irqsave(&pcdev->lock, flags); 756 757 cntl = readl(pcdev->base_emma + PRP_CNTL); 758 if (prp->cfg.channel == 1) { 759 writel(cntl & ~PRP_CNTL_CH1EN, 760 pcdev->base_emma + PRP_CNTL); 761 } else { 762 writel(cntl & ~PRP_CNTL_CH2EN, 763 pcdev->base_emma + PRP_CNTL); 764 } 765 INIT_LIST_HEAD(&pcdev->capture); 766 INIT_LIST_HEAD(&pcdev->active_bufs); 767 INIT_LIST_HEAD(&pcdev->discard); 768 769 b = pcdev->discard_buffer; 770 pcdev->discard_buffer = NULL; 771 772 spin_unlock_irqrestore(&pcdev->lock, flags); 773 774 dma_free_coherent(ici->v4l2_dev.dev, 775 pcdev->discard_size, b, pcdev->discard_buffer_dma); 776} 777 778static struct vb2_ops mx2_videobuf_ops = { 779 .queue_setup = mx2_videobuf_setup, 780 .buf_prepare = mx2_videobuf_prepare, 781 .buf_queue = mx2_videobuf_queue, 782 .start_streaming = mx2_start_streaming, 783 .stop_streaming = mx2_stop_streaming, 784}; 785 786static int mx2_camera_init_videobuf(struct vb2_queue *q, 787 struct soc_camera_device *icd) 788{ 789 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 790 q->io_modes = VB2_MMAP | VB2_USERPTR; 791 q->drv_priv = icd; 792 q->ops = &mx2_videobuf_ops; 793 q->mem_ops = &vb2_dma_contig_memops; 794 q->buf_struct_size = sizeof(struct mx2_buffer); 795 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 796 797 return vb2_queue_init(q); 798} 799 800#define MX2_BUS_FLAGS (V4L2_MBUS_MASTER | \ 801 V4L2_MBUS_VSYNC_ACTIVE_HIGH | \ 802 V4L2_MBUS_VSYNC_ACTIVE_LOW | \ 803 V4L2_MBUS_HSYNC_ACTIVE_HIGH | \ 804 V4L2_MBUS_HSYNC_ACTIVE_LOW | \ 805 V4L2_MBUS_PCLK_SAMPLE_RISING | \ 806 V4L2_MBUS_PCLK_SAMPLE_FALLING | \ 807 V4L2_MBUS_DATA_ACTIVE_HIGH | \ 808 V4L2_MBUS_DATA_ACTIVE_LOW) 809 810static int mx27_camera_emma_prp_reset(struct mx2_camera_dev *pcdev) 811{ 812 int count = 0; 813 814 readl(pcdev->base_emma + PRP_CNTL); 815 writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL); 816 while (count++ < 100) { 817 if (!(readl(pcdev->base_emma + PRP_CNTL) & PRP_CNTL_SWRST)) 818 return 0; 819 barrier(); 820 udelay(1); 821 } 822 823 return -ETIMEDOUT; 824} 825 826static int mx2_camera_set_bus_param(struct soc_camera_device *icd) 827{ 828 struct v4l2_subdev *sd = soc_camera_to_subdev(icd); 829 struct soc_camera_host *ici = to_soc_camera_host(icd->parent); 830 struct mx2_camera_dev *pcdev = ici->priv; 831 struct v4l2_mbus_config cfg = {.type = V4L2_MBUS_PARALLEL,}; 832 unsigned long common_flags; 833 int ret; 834 int bytesperline; 835 u32 csicr1 = pcdev->csicr1; 836 837 ret = v4l2_subdev_call(sd, video, g_mbus_config, &cfg); 838 if (!ret) { 839 common_flags = soc_mbus_config_compatible(&cfg, MX2_BUS_FLAGS); 840 if (!common_flags) { 841 dev_warn(icd->parent, 842 "Flags incompatible: camera 0x%x, host 0x%x\n", 843 cfg.flags, MX2_BUS_FLAGS); 844 return -EINVAL; 845 } 846 } else if (ret != -ENOIOCTLCMD) { 847 return ret; 848 } else { 849 common_flags = MX2_BUS_FLAGS; 850 } 851 852 if ((common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) && 853 (common_flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)) { 854 if (pcdev->platform_flags & MX2_CAMERA_HSYNC_HIGH) 855 common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_LOW; 856 else 857 common_flags &= ~V4L2_MBUS_HSYNC_ACTIVE_HIGH; 858 } 859 860 if ((common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) && 861 (common_flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)) { 862 if (pcdev->platform_flags & MX2_CAMERA_PCLK_SAMPLE_RISING) 863 common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_FALLING; 864 else 865 common_flags &= ~V4L2_MBUS_PCLK_SAMPLE_RISING; 866 } 867 868 cfg.flags = common_flags; 869 ret = v4l2_subdev_call(sd, video, s_mbus_config, &cfg); 870 if (ret < 0 && ret != -ENOIOCTLCMD) { 871 dev_dbg(icd->parent, "camera s_mbus_config(0x%lx) returned %d\n", 872 common_flags, ret); 873 return ret; 874 } 875 876 csicr1 = (csicr1 & ~CSICR1_FMT_MASK) | pcdev->emma_prp->cfg.csicr1; 877 878 if (common_flags & V4L2_MBUS_PCLK_SAMPLE_RISING) 879 csicr1 |= CSICR1_REDGE; 880 if (common_flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH) 881 csicr1 |= CSICR1_SOF_POL; 882 if (common_flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH) 883 csicr1 |= CSICR1_HSYNC_POL; 884 if (pcdev->platform_flags & MX2_CAMERA_EXT_VSYNC) 885 csicr1 |= CSICR1_EXT_VSYNC; 886 if (pcdev->platform_flags & MX2_CAMERA_CCIR) 887 csicr1 |= CSICR1_CCIR_EN; 888 if (pcdev->platform_flags & MX2_CAMERA_CCIR_INTERLACE) 889 csicr1 |= CSICR1_CCIR_MODE; 890 if (pcdev->platform_flags & MX2_CAMERA_GATED_CLOCK) 891 csicr1 |= CSICR1_GCLK_MODE; 892 if (pcdev->platform_flags & MX2_CAMERA_INV_DATA) 893 csicr1 |= CSICR1_INV_DATA; 894 895 pcdev->csicr1 = csicr1; 896 897 bytesperline = soc_mbus_bytes_per_line(icd->user_width, 898 icd->current_fmt->host_fmt); 899 if (bytesperline < 0) 900 return bytesperline; 901 902 ret = mx27_camera_emma_prp_reset(pcdev); 903 if (ret) 904 return ret; 905 906 writel(pcdev->csicr1, pcdev->base_csi + CSICR1); 907 908 return 0; 909} 910 911static int mx2_camera_set_crop(struct soc_camera_device *icd, 912 const struct v4l2_crop *a) 913{ 914 struct v4l2_crop a_writable = *a; 915 struct v4l2_rect *rect = &a_writable.c; 916 struct v4l2_subdev *sd = soc_camera_to_subdev(icd); 917 struct v4l2_subdev_format fmt = { 918 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 919 }; 920 struct v4l2_mbus_framefmt *mf = &fmt.format; 921 int ret; 922 923 soc_camera_limit_side(&rect->left, &rect->width, 0, 2, 4096); 924 soc_camera_limit_side(&rect->top, &rect->height, 0, 2, 4096); 925 926 ret = v4l2_subdev_call(sd, video, s_crop, a); 927 if (ret < 0) 928 return ret; 929 930 /* The capture device might have changed its output */ 931 ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &fmt); 932 if (ret < 0) 933 return ret; 934 935 dev_dbg(icd->parent, "Sensor cropped %dx%d\n", 936 mf->width, mf->height); 937 938 icd->user_width = mf->width; 939 icd->user_height = mf->height; 940 941 return ret; 942} 943 944static int mx2_camera_get_formats(struct soc_camera_device *icd, 945 unsigned int idx, 946 struct soc_camera_format_xlate *xlate) 947{ 948 struct v4l2_subdev *sd = soc_camera_to_subdev(icd); 949 const struct soc_mbus_pixelfmt *fmt; 950 struct device *dev = icd->parent; 951 struct v4l2_subdev_mbus_code_enum code = { 952 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 953 .index = idx, 954 }; 955 int ret, formats = 0; 956 957 ret = v4l2_subdev_call(sd, pad, enum_mbus_code, NULL, &code); 958 if (ret < 0) 959 /* no more formats */ 960 return 0; 961 962 fmt = soc_mbus_get_fmtdesc(code.code); 963 if (!fmt) { 964 dev_err(dev, "Invalid format code #%u: %d\n", idx, code.code); 965 return 0; 966 } 967 968 if (code.code == MEDIA_BUS_FMT_YUYV8_2X8 || 969 code.code == MEDIA_BUS_FMT_UYVY8_2X8) { 970 formats++; 971 if (xlate) { 972 /* 973 * CH2 can output YUV420 which is a standard format in 974 * soc_mediabus.c 975 */ 976 xlate->host_fmt = 977 soc_mbus_get_fmtdesc(MEDIA_BUS_FMT_YUYV8_1_5X8); 978 xlate->code = code.code; 979 dev_dbg(dev, "Providing host format %s for sensor code %d\n", 980 xlate->host_fmt->name, code.code); 981 xlate++; 982 } 983 } 984 985 if (code.code == MEDIA_BUS_FMT_UYVY8_2X8) { 986 formats++; 987 if (xlate) { 988 xlate->host_fmt = 989 soc_mbus_get_fmtdesc(MEDIA_BUS_FMT_YUYV8_2X8); 990 xlate->code = code.code; 991 dev_dbg(dev, "Providing host format %s for sensor code %d\n", 992 xlate->host_fmt->name, code.code); 993 xlate++; 994 } 995 } 996 997 /* Generic pass-trough */ 998 formats++; 999 if (xlate) { 1000 xlate->host_fmt = fmt; 1001 xlate->code = code.code; 1002 xlate++; 1003 } 1004 return formats; 1005} 1006 1007static int mx2_emmaprp_resize(struct mx2_camera_dev *pcdev, 1008 struct v4l2_mbus_framefmt *mf_in, 1009 struct v4l2_pix_format *pix_out, bool apply) 1010{ 1011 unsigned int num, den; 1012 unsigned long m; 1013 int i, dir; 1014 1015 for (dir = RESIZE_DIR_H; dir <= RESIZE_DIR_V; dir++) { 1016 struct emma_prp_resize tmprsz; 1017 unsigned char *s = tmprsz.s; 1018 int len = 0; 1019 int in, out; 1020 1021 if (dir == RESIZE_DIR_H) { 1022 in = mf_in->width; 1023 out = pix_out->width; 1024 } else { 1025 in = mf_in->height; 1026 out = pix_out->height; 1027 } 1028 1029 if (in < out) 1030 return -EINVAL; 1031 else if (in == out) 1032 continue; 1033 1034 /* Calculate ratio */ 1035 m = gcd(in, out); 1036 num = in / m; 1037 den = out / m; 1038 if (num > RESIZE_NUM_MAX) 1039 return -EINVAL; 1040 1041 if ((num >= 2 * den) && (den == 1) && 1042 (num < 9) && (!(num & 0x01))) { 1043 int sum = 0; 1044 int j; 1045 1046 /* Average scaling for >= 2:1 ratios */ 1047 /* Support can be added for num >=9 and odd values */ 1048 1049 tmprsz.algo = RESIZE_ALGO_AVERAGING; 1050 len = num; 1051 1052 for (i = 0; i < (len / 2); i++) 1053 s[i] = 8; 1054 1055 do { 1056 for (i = 0; i < (len / 2); i++) { 1057 s[i] = s[i] >> 1; 1058 sum = 0; 1059 for (j = 0; j < (len / 2); j++) 1060 sum += s[j]; 1061 if (sum == 4) 1062 break; 1063 } 1064 } while (sum != 4); 1065 1066 for (i = (len / 2); i < len; i++) 1067 s[i] = s[len - i - 1]; 1068 1069 s[len - 1] |= SZ_COEF; 1070 } else { 1071 /* bilinear scaling for < 2:1 ratios */ 1072 int v; /* overflow counter */ 1073 int coeff, nxt; /* table output */ 1074 int in_pos_inc = 2 * den; 1075 int out_pos = num; 1076 int out_pos_inc = 2 * num; 1077 int init_carry = num - den; 1078 int carry = init_carry; 1079 1080 tmprsz.algo = RESIZE_ALGO_BILINEAR; 1081 v = den + in_pos_inc; 1082 do { 1083 coeff = v - out_pos; 1084 out_pos += out_pos_inc; 1085 carry += out_pos_inc; 1086 for (nxt = 0; v < out_pos; nxt++) { 1087 v += in_pos_inc; 1088 carry -= in_pos_inc; 1089 } 1090 1091 if (len > RESIZE_NUM_MAX) 1092 return -EINVAL; 1093 1094 coeff = ((coeff << BC_COEF) + 1095 (in_pos_inc >> 1)) / in_pos_inc; 1096 1097 if (coeff >= (SZ_COEF - 1)) 1098 coeff--; 1099 1100 coeff |= SZ_COEF; 1101 s[len] = (unsigned char)coeff; 1102 len++; 1103 1104 for (i = 1; i < nxt; i++) { 1105 if (len >= RESIZE_NUM_MAX) 1106 return -EINVAL; 1107 s[len] = 0; 1108 len++; 1109 } 1110 } while (carry != init_carry); 1111 } 1112 tmprsz.len = len; 1113 if (dir == RESIZE_DIR_H) 1114 mf_in->width = pix_out->width; 1115 else 1116 mf_in->height = pix_out->height; 1117 1118 if (apply) 1119 memcpy(&pcdev->resizing[dir], &tmprsz, sizeof(tmprsz)); 1120 } 1121 return 0; 1122} 1123 1124static int mx2_camera_set_fmt(struct soc_camera_device *icd, 1125 struct v4l2_format *f) 1126{ 1127 struct soc_camera_host *ici = to_soc_camera_host(icd->parent); 1128 struct mx2_camera_dev *pcdev = ici->priv; 1129 struct v4l2_subdev *sd = soc_camera_to_subdev(icd); 1130 const struct soc_camera_format_xlate *xlate; 1131 struct v4l2_pix_format *pix = &f->fmt.pix; 1132 struct v4l2_subdev_format format = { 1133 .which = V4L2_SUBDEV_FORMAT_ACTIVE, 1134 }; 1135 struct v4l2_mbus_framefmt *mf = &format.format; 1136 int ret; 1137 1138 dev_dbg(icd->parent, "%s: requested params: width = %d, height = %d\n", 1139 __func__, pix->width, pix->height); 1140 1141 xlate = soc_camera_xlate_by_fourcc(icd, pix->pixelformat); 1142 if (!xlate) { 1143 dev_warn(icd->parent, "Format %x not found\n", 1144 pix->pixelformat); 1145 return -EINVAL; 1146 } 1147 1148 mf->width = pix->width; 1149 mf->height = pix->height; 1150 mf->field = pix->field; 1151 mf->colorspace = pix->colorspace; 1152 mf->code = xlate->code; 1153 1154 ret = v4l2_subdev_call(sd, pad, set_fmt, NULL, &format); 1155 if (ret < 0 && ret != -ENOIOCTLCMD) 1156 return ret; 1157 1158 /* Store width and height returned by the sensor for resizing */ 1159 pcdev->s_width = mf->width; 1160 pcdev->s_height = mf->height; 1161 dev_dbg(icd->parent, "%s: sensor params: width = %d, height = %d\n", 1162 __func__, pcdev->s_width, pcdev->s_height); 1163 1164 pcdev->emma_prp = mx27_emma_prp_get_format(xlate->code, 1165 xlate->host_fmt->fourcc); 1166 1167 memset(pcdev->resizing, 0, sizeof(pcdev->resizing)); 1168 if ((mf->width != pix->width || mf->height != pix->height) && 1169 pcdev->emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) { 1170 if (mx2_emmaprp_resize(pcdev, mf, pix, true) < 0) 1171 dev_dbg(icd->parent, "%s: can't resize\n", __func__); 1172 } 1173 1174 if (mf->code != xlate->code) 1175 return -EINVAL; 1176 1177 pix->width = mf->width; 1178 pix->height = mf->height; 1179 pix->field = mf->field; 1180 pix->colorspace = mf->colorspace; 1181 icd->current_fmt = xlate; 1182 1183 dev_dbg(icd->parent, "%s: returned params: width = %d, height = %d\n", 1184 __func__, pix->width, pix->height); 1185 1186 return 0; 1187} 1188 1189static int mx2_camera_try_fmt(struct soc_camera_device *icd, 1190 struct v4l2_format *f) 1191{ 1192 struct v4l2_subdev *sd = soc_camera_to_subdev(icd); 1193 const struct soc_camera_format_xlate *xlate; 1194 struct v4l2_pix_format *pix = &f->fmt.pix; 1195 struct v4l2_subdev_pad_config pad_cfg; 1196 struct v4l2_subdev_format format = { 1197 .which = V4L2_SUBDEV_FORMAT_TRY, 1198 }; 1199 struct v4l2_mbus_framefmt *mf = &format.format; 1200 __u32 pixfmt = pix->pixelformat; 1201 struct soc_camera_host *ici = to_soc_camera_host(icd->parent); 1202 struct mx2_camera_dev *pcdev = ici->priv; 1203 struct mx2_fmt_cfg *emma_prp; 1204 int ret; 1205 1206 dev_dbg(icd->parent, "%s: requested params: width = %d, height = %d\n", 1207 __func__, pix->width, pix->height); 1208 1209 xlate = soc_camera_xlate_by_fourcc(icd, pixfmt); 1210 if (pixfmt && !xlate) { 1211 dev_warn(icd->parent, "Format %x not found\n", pixfmt); 1212 return -EINVAL; 1213 } 1214 1215 /* 1216 * limit to MX27 hardware capabilities: width must be a multiple of 8 as 1217 * requested by the CSI. (Table 39-2 in the i.MX27 Reference Manual). 1218 */ 1219 pix->width &= ~0x7; 1220 1221 /* limit to sensor capabilities */ 1222 mf->width = pix->width; 1223 mf->height = pix->height; 1224 mf->field = pix->field; 1225 mf->colorspace = pix->colorspace; 1226 mf->code = xlate->code; 1227 1228 ret = v4l2_subdev_call(sd, pad, set_fmt, &pad_cfg, &format); 1229 if (ret < 0) 1230 return ret; 1231 1232 dev_dbg(icd->parent, "%s: sensor params: width = %d, height = %d\n", 1233 __func__, pcdev->s_width, pcdev->s_height); 1234 1235 /* If the sensor does not support image size try PrP resizing */ 1236 emma_prp = mx27_emma_prp_get_format(xlate->code, 1237 xlate->host_fmt->fourcc); 1238 1239 if ((mf->width != pix->width || mf->height != pix->height) && 1240 emma_prp->cfg.in_fmt == PRP_CNTL_DATA_IN_YUV422) { 1241 if (mx2_emmaprp_resize(pcdev, mf, pix, false) < 0) 1242 dev_dbg(icd->parent, "%s: can't resize\n", __func__); 1243 } 1244 1245 if (mf->field == V4L2_FIELD_ANY) 1246 mf->field = V4L2_FIELD_NONE; 1247 /* 1248 * Driver supports interlaced images provided they have 1249 * both fields so that they can be processed as if they 1250 * were progressive. 1251 */ 1252 if (mf->field != V4L2_FIELD_NONE && !V4L2_FIELD_HAS_BOTH(mf->field)) { 1253 dev_err(icd->parent, "Field type %d unsupported.\n", 1254 mf->field); 1255 return -EINVAL; 1256 } 1257 1258 pix->width = mf->width; 1259 pix->height = mf->height; 1260 pix->field = mf->field; 1261 pix->colorspace = mf->colorspace; 1262 1263 dev_dbg(icd->parent, "%s: returned params: width = %d, height = %d\n", 1264 __func__, pix->width, pix->height); 1265 1266 return 0; 1267} 1268 1269static int mx2_camera_querycap(struct soc_camera_host *ici, 1270 struct v4l2_capability *cap) 1271{ 1272 /* cap->name is set by the friendly caller:-> */ 1273 strlcpy(cap->card, MX2_CAM_DRIVER_DESCRIPTION, sizeof(cap->card)); 1274 cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING; 1275 cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS; 1276 1277 return 0; 1278} 1279 1280static unsigned int mx2_camera_poll(struct file *file, poll_table *pt) 1281{ 1282 struct soc_camera_device *icd = file->private_data; 1283 1284 return vb2_poll(&icd->vb2_vidq, file, pt); 1285} 1286 1287static struct soc_camera_host_ops mx2_soc_camera_host_ops = { 1288 .owner = THIS_MODULE, 1289 .add = mx2_camera_add_device, 1290 .remove = mx2_camera_remove_device, 1291 .clock_start = mx2_camera_clock_start, 1292 .clock_stop = mx2_camera_clock_stop, 1293 .set_fmt = mx2_camera_set_fmt, 1294 .set_crop = mx2_camera_set_crop, 1295 .get_formats = mx2_camera_get_formats, 1296 .try_fmt = mx2_camera_try_fmt, 1297 .init_videobuf2 = mx2_camera_init_videobuf, 1298 .poll = mx2_camera_poll, 1299 .querycap = mx2_camera_querycap, 1300 .set_bus_param = mx2_camera_set_bus_param, 1301}; 1302 1303static void mx27_camera_frame_done_emma(struct mx2_camera_dev *pcdev, 1304 int bufnum, bool err) 1305{ 1306#ifdef DEBUG 1307 struct mx2_fmt_cfg *prp = pcdev->emma_prp; 1308#endif 1309 struct mx2_buf_internal *ibuf; 1310 struct mx2_buffer *buf; 1311 struct vb2_buffer *vb; 1312 struct vb2_v4l2_buffer *vbuf; 1313 unsigned long phys; 1314 1315 ibuf = list_first_entry(&pcdev->active_bufs, struct mx2_buf_internal, 1316 queue); 1317 1318 BUG_ON(ibuf->bufnum != bufnum); 1319 1320 if (ibuf->discard) { 1321 /* 1322 * Discard buffer must not be returned to user space. 1323 * Just return it to the discard queue. 1324 */ 1325 list_move_tail(pcdev->active_bufs.next, &pcdev->discard); 1326 } else { 1327 buf = mx2_ibuf_to_buf(ibuf); 1328 1329 vb = &buf->vb.vb2_buf; 1330 vbuf = to_vb2_v4l2_buffer(vb); 1331#ifdef DEBUG 1332 phys = vb2_dma_contig_plane_dma_addr(vb, 0); 1333 if (prp->cfg.channel == 1) { 1334 if (readl(pcdev->base_emma + PRP_DEST_RGB1_PTR + 1335 4 * bufnum) != phys) { 1336 dev_err(pcdev->dev, "%lx != %x\n", phys, 1337 readl(pcdev->base_emma + 1338 PRP_DEST_RGB1_PTR + 4 * bufnum)); 1339 } 1340 } else { 1341 if (readl(pcdev->base_emma + PRP_DEST_Y_PTR - 1342 0x14 * bufnum) != phys) { 1343 dev_err(pcdev->dev, "%lx != %x\n", phys, 1344 readl(pcdev->base_emma + 1345 PRP_DEST_Y_PTR - 0x14 * bufnum)); 1346 } 1347 } 1348#endif 1349 dev_dbg(pcdev->dev, "%s (vb=0x%p) 0x%p %lu\n", __func__, vb, 1350 vb2_plane_vaddr(vb, 0), 1351 vb2_get_plane_payload(vb, 0)); 1352 1353 list_del_init(&buf->internal.queue); 1354 v4l2_get_timestamp(&vbuf->timestamp); 1355 vbuf->sequence = pcdev->frame_count; 1356 if (err) 1357 vb2_buffer_done(vb, VB2_BUF_STATE_ERROR); 1358 else 1359 vb2_buffer_done(vb, VB2_BUF_STATE_DONE); 1360 } 1361 1362 pcdev->frame_count++; 1363 1364 if (list_empty(&pcdev->capture)) { 1365 if (list_empty(&pcdev->discard)) { 1366 dev_warn(pcdev->dev, "%s: trying to access empty discard list\n", 1367 __func__); 1368 return; 1369 } 1370 1371 ibuf = list_first_entry(&pcdev->discard, 1372 struct mx2_buf_internal, queue); 1373 ibuf->bufnum = bufnum; 1374 1375 list_move_tail(pcdev->discard.next, &pcdev->active_bufs); 1376 mx27_update_emma_buf(pcdev, pcdev->discard_buffer_dma, bufnum); 1377 return; 1378 } 1379 1380 buf = list_first_entry(&pcdev->capture, struct mx2_buffer, 1381 internal.queue); 1382 1383 buf->internal.bufnum = bufnum; 1384 1385 list_move_tail(pcdev->capture.next, &pcdev->active_bufs); 1386 1387 vb = &buf->vb.vb2_buf; 1388 1389 phys = vb2_dma_contig_plane_dma_addr(vb, 0); 1390 mx27_update_emma_buf(pcdev, phys, bufnum); 1391} 1392 1393static irqreturn_t mx27_camera_emma_irq(int irq_emma, void *data) 1394{ 1395 struct mx2_camera_dev *pcdev = data; 1396 unsigned int status = readl(pcdev->base_emma + PRP_INTRSTATUS); 1397 struct mx2_buf_internal *ibuf; 1398 1399 spin_lock(&pcdev->lock); 1400 1401 if (list_empty(&pcdev->active_bufs)) { 1402 dev_warn(pcdev->dev, "%s: called while active list is empty\n", 1403 __func__); 1404 1405 if (!status) { 1406 spin_unlock(&pcdev->lock); 1407 return IRQ_NONE; 1408 } 1409 } 1410 1411 if (status & (1 << 7)) { /* overflow */ 1412 u32 cntl = readl(pcdev->base_emma + PRP_CNTL); 1413 writel(cntl & ~(PRP_CNTL_CH1EN | PRP_CNTL_CH2EN), 1414 pcdev->base_emma + PRP_CNTL); 1415 writel(cntl, pcdev->base_emma + PRP_CNTL); 1416 1417 ibuf = list_first_entry(&pcdev->active_bufs, 1418 struct mx2_buf_internal, queue); 1419 mx27_camera_frame_done_emma(pcdev, 1420 ibuf->bufnum, true); 1421 1422 status &= ~(1 << 7); 1423 } else if (((status & (3 << 5)) == (3 << 5)) || 1424 ((status & (3 << 3)) == (3 << 3))) { 1425 /* 1426 * Both buffers have triggered, process the one we're expecting 1427 * to first 1428 */ 1429 ibuf = list_first_entry(&pcdev->active_bufs, 1430 struct mx2_buf_internal, queue); 1431 mx27_camera_frame_done_emma(pcdev, ibuf->bufnum, false); 1432 status &= ~(1 << (6 - ibuf->bufnum)); /* mark processed */ 1433 } else if ((status & (1 << 6)) || (status & (1 << 4))) { 1434 mx27_camera_frame_done_emma(pcdev, 0, false); 1435 } else if ((status & (1 << 5)) || (status & (1 << 3))) { 1436 mx27_camera_frame_done_emma(pcdev, 1, false); 1437 } 1438 1439 spin_unlock(&pcdev->lock); 1440 writel(status, pcdev->base_emma + PRP_INTRSTATUS); 1441 1442 return IRQ_HANDLED; 1443} 1444 1445static int mx27_camera_emma_init(struct platform_device *pdev) 1446{ 1447 struct mx2_camera_dev *pcdev = platform_get_drvdata(pdev); 1448 struct resource *res_emma; 1449 int irq_emma; 1450 int err = 0; 1451 1452 res_emma = platform_get_resource(pdev, IORESOURCE_MEM, 1); 1453 irq_emma = platform_get_irq(pdev, 1); 1454 if (!res_emma || !irq_emma) { 1455 dev_err(pcdev->dev, "no EMMA resources\n"); 1456 err = -ENODEV; 1457 goto out; 1458 } 1459 1460 pcdev->base_emma = devm_ioremap_resource(pcdev->dev, res_emma); 1461 if (IS_ERR(pcdev->base_emma)) { 1462 err = PTR_ERR(pcdev->base_emma); 1463 goto out; 1464 } 1465 1466 err = devm_request_irq(pcdev->dev, irq_emma, mx27_camera_emma_irq, 0, 1467 MX2_CAM_DRV_NAME, pcdev); 1468 if (err) { 1469 dev_err(pcdev->dev, "Camera EMMA interrupt register failed\n"); 1470 goto out; 1471 } 1472 1473 pcdev->clk_emma_ipg = devm_clk_get(pcdev->dev, "emma-ipg"); 1474 if (IS_ERR(pcdev->clk_emma_ipg)) { 1475 err = PTR_ERR(pcdev->clk_emma_ipg); 1476 goto out; 1477 } 1478 1479 clk_prepare_enable(pcdev->clk_emma_ipg); 1480 1481 pcdev->clk_emma_ahb = devm_clk_get(pcdev->dev, "emma-ahb"); 1482 if (IS_ERR(pcdev->clk_emma_ahb)) { 1483 err = PTR_ERR(pcdev->clk_emma_ahb); 1484 goto exit_clk_emma_ipg; 1485 } 1486 1487 clk_prepare_enable(pcdev->clk_emma_ahb); 1488 1489 err = mx27_camera_emma_prp_reset(pcdev); 1490 if (err) 1491 goto exit_clk_emma_ahb; 1492 1493 return err; 1494 1495exit_clk_emma_ahb: 1496 clk_disable_unprepare(pcdev->clk_emma_ahb); 1497exit_clk_emma_ipg: 1498 clk_disable_unprepare(pcdev->clk_emma_ipg); 1499out: 1500 return err; 1501} 1502 1503static int mx2_camera_probe(struct platform_device *pdev) 1504{ 1505 struct mx2_camera_dev *pcdev; 1506 struct resource *res_csi; 1507 int irq_csi; 1508 int err = 0; 1509 1510 dev_dbg(&pdev->dev, "initialising\n"); 1511 1512 res_csi = platform_get_resource(pdev, IORESOURCE_MEM, 0); 1513 irq_csi = platform_get_irq(pdev, 0); 1514 if (res_csi == NULL || irq_csi < 0) { 1515 dev_err(&pdev->dev, "Missing platform resources data\n"); 1516 err = -ENODEV; 1517 goto exit; 1518 } 1519 1520 pcdev = devm_kzalloc(&pdev->dev, sizeof(*pcdev), GFP_KERNEL); 1521 if (!pcdev) { 1522 dev_err(&pdev->dev, "Could not allocate pcdev\n"); 1523 err = -ENOMEM; 1524 goto exit; 1525 } 1526 1527 pcdev->clk_csi_ahb = devm_clk_get(&pdev->dev, "ahb"); 1528 if (IS_ERR(pcdev->clk_csi_ahb)) { 1529 dev_err(&pdev->dev, "Could not get csi ahb clock\n"); 1530 err = PTR_ERR(pcdev->clk_csi_ahb); 1531 goto exit; 1532 } 1533 1534 pcdev->clk_csi_per = devm_clk_get(&pdev->dev, "per"); 1535 if (IS_ERR(pcdev->clk_csi_per)) { 1536 dev_err(&pdev->dev, "Could not get csi per clock\n"); 1537 err = PTR_ERR(pcdev->clk_csi_per); 1538 goto exit; 1539 } 1540 1541 pcdev->pdata = pdev->dev.platform_data; 1542 if (pcdev->pdata) { 1543 long rate; 1544 1545 pcdev->platform_flags = pcdev->pdata->flags; 1546 1547 rate = clk_round_rate(pcdev->clk_csi_per, 1548 pcdev->pdata->clk * 2); 1549 if (rate <= 0) { 1550 err = -ENODEV; 1551 goto exit; 1552 } 1553 err = clk_set_rate(pcdev->clk_csi_per, rate); 1554 if (err < 0) 1555 goto exit; 1556 } 1557 1558 INIT_LIST_HEAD(&pcdev->capture); 1559 INIT_LIST_HEAD(&pcdev->active_bufs); 1560 INIT_LIST_HEAD(&pcdev->discard); 1561 spin_lock_init(&pcdev->lock); 1562 1563 pcdev->base_csi = devm_ioremap_resource(&pdev->dev, res_csi); 1564 if (IS_ERR(pcdev->base_csi)) { 1565 err = PTR_ERR(pcdev->base_csi); 1566 goto exit; 1567 } 1568 1569 pcdev->dev = &pdev->dev; 1570 platform_set_drvdata(pdev, pcdev); 1571 1572 err = mx27_camera_emma_init(pdev); 1573 if (err) 1574 goto exit; 1575 1576 /* 1577 * We're done with drvdata here. Clear the pointer so that 1578 * v4l2 core can start using drvdata on its purpose. 1579 */ 1580 platform_set_drvdata(pdev, NULL); 1581 1582 pcdev->soc_host.drv_name = MX2_CAM_DRV_NAME, 1583 pcdev->soc_host.ops = &mx2_soc_camera_host_ops, 1584 pcdev->soc_host.priv = pcdev; 1585 pcdev->soc_host.v4l2_dev.dev = &pdev->dev; 1586 pcdev->soc_host.nr = pdev->id; 1587 1588 pcdev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev); 1589 if (IS_ERR(pcdev->alloc_ctx)) { 1590 err = PTR_ERR(pcdev->alloc_ctx); 1591 goto eallocctx; 1592 } 1593 err = soc_camera_host_register(&pcdev->soc_host); 1594 if (err) 1595 goto exit_free_emma; 1596 1597 dev_info(&pdev->dev, "MX2 Camera (CSI) driver probed, clock frequency: %ld\n", 1598 clk_get_rate(pcdev->clk_csi_per)); 1599 1600 return 0; 1601 1602exit_free_emma: 1603 vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx); 1604eallocctx: 1605 clk_disable_unprepare(pcdev->clk_emma_ipg); 1606 clk_disable_unprepare(pcdev->clk_emma_ahb); 1607exit: 1608 return err; 1609} 1610 1611static int mx2_camera_remove(struct platform_device *pdev) 1612{ 1613 struct soc_camera_host *soc_host = to_soc_camera_host(&pdev->dev); 1614 struct mx2_camera_dev *pcdev = container_of(soc_host, 1615 struct mx2_camera_dev, soc_host); 1616 1617 soc_camera_host_unregister(&pcdev->soc_host); 1618 1619 vb2_dma_contig_cleanup_ctx(pcdev->alloc_ctx); 1620 1621 clk_disable_unprepare(pcdev->clk_emma_ipg); 1622 clk_disable_unprepare(pcdev->clk_emma_ahb); 1623 1624 dev_info(&pdev->dev, "MX2 Camera driver unloaded\n"); 1625 1626 return 0; 1627} 1628 1629static struct platform_driver mx2_camera_driver = { 1630 .driver = { 1631 .name = MX2_CAM_DRV_NAME, 1632 }, 1633 .id_table = mx2_camera_devtype, 1634 .remove = mx2_camera_remove, 1635}; 1636 1637module_platform_driver_probe(mx2_camera_driver, mx2_camera_probe); 1638 1639MODULE_DESCRIPTION("i.MX27 SoC Camera Host driver"); 1640MODULE_AUTHOR("Sascha Hauer <sha@pengutronix.de>"); 1641MODULE_LICENSE("GPL"); 1642MODULE_VERSION(MX2_CAM_VERSION); 1643