1310 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * Copyright (C) 2015 VanguardiaSur - www.vanguardiasur.com.ar4 *5 * Based on original driver by Krzysztof Ha?asa:6 * Copyright (C) 2015 Industrial Research Institute for Automation7 * and Measurements PIAP8 */9 10#include <linux/init.h>11#include <linux/delay.h>12#include <linux/list.h>13#include <linux/module.h>14#include <linux/kernel.h>15#include <linux/slab.h>16#include <media/v4l2-common.h>17#include <media/v4l2-event.h>18#include <media/videobuf2-dma-contig.h>19#include <media/videobuf2-dma-sg.h>20#include <media/videobuf2-vmalloc.h>21#include "tw686x.h"22#include "tw686x-regs.h"23 24#define TW686X_INPUTS_PER_CH 425#define TW686X_VIDEO_WIDTH 72026#define TW686X_VIDEO_HEIGHT(id) ((id & V4L2_STD_525_60) ? 480 : 576)27#define TW686X_MAX_FPS(id) ((id & V4L2_STD_525_60) ? 30 : 25)28 29#define TW686X_MAX_SG_ENTRY_SIZE 409630#define TW686X_MAX_SG_DESC_COUNT 256 /* PAL 720x576 needs 203 4-KB pages */31#define TW686X_SG_TABLE_SIZE (TW686X_MAX_SG_DESC_COUNT * sizeof(struct tw686x_sg_desc))32 33static const struct tw686x_format formats[] = {34 {35 .fourcc = V4L2_PIX_FMT_UYVY,36 .mode = 0,37 .depth = 16,38 }, {39 .fourcc = V4L2_PIX_FMT_RGB565,40 .mode = 5,41 .depth = 16,42 }, {43 .fourcc = V4L2_PIX_FMT_YUYV,44 .mode = 6,45 .depth = 16,46 }47};48 49static void tw686x_buf_done(struct tw686x_video_channel *vc,50 unsigned int pb)51{52 struct tw686x_dma_desc *desc = &vc->dma_descs[pb];53 struct tw686x_dev *dev = vc->dev;54 struct vb2_v4l2_buffer *vb;55 struct vb2_buffer *vb2_buf;56 57 if (vc->curr_bufs[pb]) {58 vb = &vc->curr_bufs[pb]->vb;59 60 vb->field = dev->dma_ops->field;61 vb->sequence = vc->sequence++;62 vb2_buf = &vb->vb2_buf;63 64 if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY)65 memcpy(vb2_plane_vaddr(vb2_buf, 0), desc->virt,66 desc->size);67 vb2_buf->timestamp = ktime_get_ns();68 vb2_buffer_done(vb2_buf, VB2_BUF_STATE_DONE);69 }70 71 vc->pb = !pb;72}73 74/*75 * We can call this even when alloc_dma failed for the given channel76 */77static void tw686x_memcpy_dma_free(struct tw686x_video_channel *vc,78 unsigned int pb)79{80 struct tw686x_dma_desc *desc = &vc->dma_descs[pb];81 struct tw686x_dev *dev = vc->dev;82 struct pci_dev *pci_dev;83 unsigned long flags;84 85 /* Check device presence. Shouldn't really happen! */86 spin_lock_irqsave(&dev->lock, flags);87 pci_dev = dev->pci_dev;88 spin_unlock_irqrestore(&dev->lock, flags);89 if (!pci_dev) {90 WARN(1, "trying to deallocate on missing device\n");91 return;92 }93 94 if (desc->virt) {95 dma_free_coherent(&dev->pci_dev->dev, desc->size, desc->virt,96 desc->phys);97 desc->virt = NULL;98 }99}100 101static int tw686x_memcpy_dma_alloc(struct tw686x_video_channel *vc,102 unsigned int pb)103{104 struct tw686x_dev *dev = vc->dev;105 u32 reg = pb ? VDMA_B_ADDR[vc->ch] : VDMA_P_ADDR[vc->ch];106 unsigned int len;107 void *virt;108 109 WARN(vc->dma_descs[pb].virt,110 "Allocating buffer but previous still here\n");111 112 len = (vc->width * vc->height * vc->format->depth) >> 3;113 virt = dma_alloc_coherent(&dev->pci_dev->dev, len,114 &vc->dma_descs[pb].phys, GFP_KERNEL);115 if (!virt) {116 v4l2_err(&dev->v4l2_dev,117 "dma%d: unable to allocate %s-buffer\n",118 vc->ch, pb ? "B" : "P");119 return -ENOMEM;120 }121 vc->dma_descs[pb].size = len;122 vc->dma_descs[pb].virt = virt;123 reg_write(dev, reg, vc->dma_descs[pb].phys);124 125 return 0;126}127 128static void tw686x_memcpy_buf_refill(struct tw686x_video_channel *vc,129 unsigned int pb)130{131 struct tw686x_v4l2_buf *buf;132 133 while (!list_empty(&vc->vidq_queued)) {134 135 buf = list_first_entry(&vc->vidq_queued,136 struct tw686x_v4l2_buf, list);137 list_del(&buf->list);138 139 vc->curr_bufs[pb] = buf;140 return;141 }142 vc->curr_bufs[pb] = NULL;143}144 145static const struct tw686x_dma_ops memcpy_dma_ops = {146 .alloc = tw686x_memcpy_dma_alloc,147 .free = tw686x_memcpy_dma_free,148 .buf_refill = tw686x_memcpy_buf_refill,149 .mem_ops = &vb2_vmalloc_memops,150 .hw_dma_mode = TW686X_FRAME_MODE,151 .field = V4L2_FIELD_INTERLACED,152};153 154static void tw686x_contig_buf_refill(struct tw686x_video_channel *vc,155 unsigned int pb)156{157 struct tw686x_v4l2_buf *buf;158 159 while (!list_empty(&vc->vidq_queued)) {160 u32 reg = pb ? VDMA_B_ADDR[vc->ch] : VDMA_P_ADDR[vc->ch];161 dma_addr_t phys;162 163 buf = list_first_entry(&vc->vidq_queued,164 struct tw686x_v4l2_buf, list);165 list_del(&buf->list);166 167 phys = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);168 reg_write(vc->dev, reg, phys);169 170 buf->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;171 vc->curr_bufs[pb] = buf;172 return;173 }174 vc->curr_bufs[pb] = NULL;175}176 177static const struct tw686x_dma_ops contig_dma_ops = {178 .buf_refill = tw686x_contig_buf_refill,179 .mem_ops = &vb2_dma_contig_memops,180 .hw_dma_mode = TW686X_FRAME_MODE,181 .field = V4L2_FIELD_INTERLACED,182};183 184static int tw686x_sg_desc_fill(struct tw686x_sg_desc *descs,185 struct tw686x_v4l2_buf *buf,186 unsigned int buf_len)187{188 struct sg_table *vbuf = vb2_dma_sg_plane_desc(&buf->vb.vb2_buf, 0);189 unsigned int len, entry_len;190 struct scatterlist *sg;191 int i, count;192 193 /* Clear the scatter-gather table */194 memset(descs, 0, TW686X_SG_TABLE_SIZE);195 196 count = 0;197 for_each_sg(vbuf->sgl, sg, vbuf->nents, i) {198 dma_addr_t phys = sg_dma_address(sg);199 len = sg_dma_len(sg);200 201 while (len && buf_len) {202 203 if (count == TW686X_MAX_SG_DESC_COUNT)204 return -ENOMEM;205 206 entry_len = min_t(unsigned int, len,207 TW686X_MAX_SG_ENTRY_SIZE);208 entry_len = min_t(unsigned int, entry_len, buf_len);209 descs[count].phys = cpu_to_le32(phys);210 descs[count++].flags_length =211 cpu_to_le32(BIT(30) | entry_len);212 phys += entry_len;213 len -= entry_len;214 buf_len -= entry_len;215 }216 217 if (!buf_len)218 return 0;219 }220 221 return -ENOMEM;222}223 224static void tw686x_sg_buf_refill(struct tw686x_video_channel *vc,225 unsigned int pb)226{227 struct tw686x_dev *dev = vc->dev;228 struct tw686x_v4l2_buf *buf;229 230 while (!list_empty(&vc->vidq_queued)) {231 unsigned int buf_len;232 233 buf = list_first_entry(&vc->vidq_queued,234 struct tw686x_v4l2_buf, list);235 list_del(&buf->list);236 237 buf_len = (vc->width * vc->height * vc->format->depth) >> 3;238 if (tw686x_sg_desc_fill(vc->sg_descs[pb], buf, buf_len)) {239 v4l2_err(&dev->v4l2_dev,240 "dma%d: unable to fill %s-buffer\n",241 vc->ch, pb ? "B" : "P");242 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);243 continue;244 }245 246 buf->vb.vb2_buf.state = VB2_BUF_STATE_ACTIVE;247 vc->curr_bufs[pb] = buf;248 return;249 }250 251 vc->curr_bufs[pb] = NULL;252}253 254static void tw686x_sg_dma_free(struct tw686x_video_channel *vc,255 unsigned int pb)256{257 struct tw686x_dma_desc *desc = &vc->dma_descs[pb];258 struct tw686x_dev *dev = vc->dev;259 260 if (desc->size) {261 dma_free_coherent(&dev->pci_dev->dev, desc->size, desc->virt,262 desc->phys);263 desc->virt = NULL;264 }265 266 vc->sg_descs[pb] = NULL;267}268 269static int tw686x_sg_dma_alloc(struct tw686x_video_channel *vc,270 unsigned int pb)271{272 struct tw686x_dma_desc *desc = &vc->dma_descs[pb];273 struct tw686x_dev *dev = vc->dev;274 u32 reg = pb ? DMA_PAGE_TABLE1_ADDR[vc->ch] :275 DMA_PAGE_TABLE0_ADDR[vc->ch];276 void *virt;277 278 if (desc->size) {279 virt = dma_alloc_coherent(&dev->pci_dev->dev, desc->size,280 &desc->phys, GFP_KERNEL);281 if (!virt) {282 v4l2_err(&dev->v4l2_dev,283 "dma%d: unable to allocate %s-buffer\n",284 vc->ch, pb ? "B" : "P");285 return -ENOMEM;286 }287 desc->virt = virt;288 reg_write(dev, reg, desc->phys);289 } else {290 virt = dev->video_channels[0].dma_descs[pb].virt +291 vc->ch * TW686X_SG_TABLE_SIZE;292 }293 294 vc->sg_descs[pb] = virt;295 return 0;296}297 298static int tw686x_sg_setup(struct tw686x_dev *dev)299{300 unsigned int sg_table_size, pb, ch, channels;301 302 if (is_second_gen(dev)) {303 /*304 * TW6865/TW6869: each channel needs a pair of305 * P-B descriptor tables.306 */307 channels = max_channels(dev);308 sg_table_size = TW686X_SG_TABLE_SIZE;309 } else {310 /*311 * TW6864/TW6868: we need to allocate a pair of312 * P-B descriptor tables, common for all channels.313 * Each table will be bigger than 4 KB.314 */315 channels = 1;316 sg_table_size = max_channels(dev) * TW686X_SG_TABLE_SIZE;317 }318 319 for (ch = 0; ch < channels; ch++) {320 struct tw686x_video_channel *vc = &dev->video_channels[ch];321 322 for (pb = 0; pb < 2; pb++)323 vc->dma_descs[pb].size = sg_table_size;324 }325 326 return 0;327}328 329static const struct tw686x_dma_ops sg_dma_ops = {330 .setup = tw686x_sg_setup,331 .alloc = tw686x_sg_dma_alloc,332 .free = tw686x_sg_dma_free,333 .buf_refill = tw686x_sg_buf_refill,334 .mem_ops = &vb2_dma_sg_memops,335 .hw_dma_mode = TW686X_SG_MODE,336 .field = V4L2_FIELD_SEQ_TB,337};338 339static const unsigned int fps_map[15] = {340 /*341 * bit 31 enables selecting the field control register342 * bits 0-29 are a bitmask with fields that will be output.343 * For NTSC (and PAL-M, PAL-60), all 30 bits are used.344 * For other PAL standards, only the first 25 bits are used.345 */346 0x00000000, /* output all fields */347 0x80000006, /* 2 fps (60Hz), 2 fps (50Hz) */348 0x80018006, /* 4 fps (60Hz), 4 fps (50Hz) */349 0x80618006, /* 6 fps (60Hz), 6 fps (50Hz) */350 0x81818186, /* 8 fps (60Hz), 8 fps (50Hz) */351 0x86186186, /* 10 fps (60Hz), 8 fps (50Hz) */352 0x86619866, /* 12 fps (60Hz), 10 fps (50Hz) */353 0x86666666, /* 14 fps (60Hz), 12 fps (50Hz) */354 0x9999999e, /* 16 fps (60Hz), 14 fps (50Hz) */355 0x99e6799e, /* 18 fps (60Hz), 16 fps (50Hz) */356 0x9e79e79e, /* 20 fps (60Hz), 16 fps (50Hz) */357 0x9e7e7e7e, /* 22 fps (60Hz), 18 fps (50Hz) */358 0x9fe7f9fe, /* 24 fps (60Hz), 20 fps (50Hz) */359 0x9ffe7ffe, /* 26 fps (60Hz), 22 fps (50Hz) */360 0x9ffffffe, /* 28 fps (60Hz), 24 fps (50Hz) */361};362 363static unsigned int tw686x_real_fps(unsigned int index, unsigned int max_fps)364{365 unsigned long mask;366 367 if (!index || index >= ARRAY_SIZE(fps_map))368 return max_fps;369 370 mask = GENMASK(max_fps - 1, 0);371 return hweight_long(fps_map[index] & mask);372}373 374static unsigned int tw686x_fps_idx(unsigned int fps, unsigned int max_fps)375{376 unsigned int idx, real_fps;377 int delta;378 379 /* First guess */380 idx = (12 + 15 * fps) / max_fps;381 382 /* Minimal possible framerate is 2 frames per second */383 if (!idx)384 return 1;385 386 /* Check if the difference is bigger than abs(1) and adjust */387 real_fps = tw686x_real_fps(idx, max_fps);388 delta = real_fps - fps;389 if (delta < -1)390 idx++;391 else if (delta > 1)392 idx--;393 394 /* Max framerate */395 if (idx >= 15)396 return 0;397 398 return idx;399}400 401static void tw686x_set_framerate(struct tw686x_video_channel *vc,402 unsigned int fps)403{404 unsigned int i;405 406 i = tw686x_fps_idx(fps, TW686X_MAX_FPS(vc->video_standard));407 reg_write(vc->dev, VIDEO_FIELD_CTRL[vc->ch], fps_map[i]);408 vc->fps = tw686x_real_fps(i, TW686X_MAX_FPS(vc->video_standard));409}410 411static const struct tw686x_format *format_by_fourcc(unsigned int fourcc)412{413 unsigned int cnt;414 415 for (cnt = 0; cnt < ARRAY_SIZE(formats); cnt++)416 if (formats[cnt].fourcc == fourcc)417 return &formats[cnt];418 return NULL;419}420 421static int tw686x_queue_setup(struct vb2_queue *vq,422 unsigned int *nbuffers, unsigned int *nplanes,423 unsigned int sizes[], struct device *alloc_devs[])424{425 struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);426 unsigned int q_num_bufs = vb2_get_num_buffers(vq);427 unsigned int szimage =428 (vc->width * vc->height * vc->format->depth) >> 3;429 430 /*431 * Let's request at least three buffers: two for the432 * DMA engine and one for userspace.433 */434 if (q_num_bufs + *nbuffers < 3)435 *nbuffers = 3 - q_num_bufs;436 437 if (*nplanes) {438 if (*nplanes != 1 || sizes[0] < szimage)439 return -EINVAL;440 return 0;441 }442 443 sizes[0] = szimage;444 *nplanes = 1;445 return 0;446}447 448static void tw686x_buf_queue(struct vb2_buffer *vb)449{450 struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue);451 struct tw686x_dev *dev = vc->dev;452 struct pci_dev *pci_dev;453 unsigned long flags;454 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);455 struct tw686x_v4l2_buf *buf =456 container_of(vbuf, struct tw686x_v4l2_buf, vb);457 458 /* Check device presence */459 spin_lock_irqsave(&dev->lock, flags);460 pci_dev = dev->pci_dev;461 spin_unlock_irqrestore(&dev->lock, flags);462 if (!pci_dev) {463 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);464 return;465 }466 467 spin_lock_irqsave(&vc->qlock, flags);468 list_add_tail(&buf->list, &vc->vidq_queued);469 spin_unlock_irqrestore(&vc->qlock, flags);470}471 472static void tw686x_clear_queue(struct tw686x_video_channel *vc,473 enum vb2_buffer_state state)474{475 unsigned int pb;476 477 while (!list_empty(&vc->vidq_queued)) {478 struct tw686x_v4l2_buf *buf;479 480 buf = list_first_entry(&vc->vidq_queued,481 struct tw686x_v4l2_buf, list);482 list_del(&buf->list);483 vb2_buffer_done(&buf->vb.vb2_buf, state);484 }485 486 for (pb = 0; pb < 2; pb++) {487 if (vc->curr_bufs[pb])488 vb2_buffer_done(&vc->curr_bufs[pb]->vb.vb2_buf, state);489 vc->curr_bufs[pb] = NULL;490 }491}492 493static int tw686x_start_streaming(struct vb2_queue *vq, unsigned int count)494{495 struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);496 struct tw686x_dev *dev = vc->dev;497 struct pci_dev *pci_dev;498 unsigned long flags;499 int pb, err;500 501 /* Check device presence */502 spin_lock_irqsave(&dev->lock, flags);503 pci_dev = dev->pci_dev;504 spin_unlock_irqrestore(&dev->lock, flags);505 if (!pci_dev) {506 err = -ENODEV;507 goto err_clear_queue;508 }509 510 spin_lock_irqsave(&vc->qlock, flags);511 512 /* Sanity check */513 if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY &&514 (!vc->dma_descs[0].virt || !vc->dma_descs[1].virt)) {515 spin_unlock_irqrestore(&vc->qlock, flags);516 v4l2_err(&dev->v4l2_dev,517 "video%d: refusing to start without DMA buffers\n",518 vc->num);519 err = -ENOMEM;520 goto err_clear_queue;521 }522 523 for (pb = 0; pb < 2; pb++)524 dev->dma_ops->buf_refill(vc, pb);525 spin_unlock_irqrestore(&vc->qlock, flags);526 527 vc->sequence = 0;528 vc->pb = 0;529 530 spin_lock_irqsave(&dev->lock, flags);531 tw686x_enable_channel(dev, vc->ch);532 spin_unlock_irqrestore(&dev->lock, flags);533 534 mod_timer(&dev->dma_delay_timer, jiffies + msecs_to_jiffies(100));535 536 return 0;537 538err_clear_queue:539 spin_lock_irqsave(&vc->qlock, flags);540 tw686x_clear_queue(vc, VB2_BUF_STATE_QUEUED);541 spin_unlock_irqrestore(&vc->qlock, flags);542 return err;543}544 545static void tw686x_stop_streaming(struct vb2_queue *vq)546{547 struct tw686x_video_channel *vc = vb2_get_drv_priv(vq);548 struct tw686x_dev *dev = vc->dev;549 struct pci_dev *pci_dev;550 unsigned long flags;551 552 /* Check device presence */553 spin_lock_irqsave(&dev->lock, flags);554 pci_dev = dev->pci_dev;555 spin_unlock_irqrestore(&dev->lock, flags);556 if (pci_dev)557 tw686x_disable_channel(dev, vc->ch);558 559 spin_lock_irqsave(&vc->qlock, flags);560 tw686x_clear_queue(vc, VB2_BUF_STATE_ERROR);561 spin_unlock_irqrestore(&vc->qlock, flags);562}563 564static int tw686x_buf_prepare(struct vb2_buffer *vb)565{566 struct tw686x_video_channel *vc = vb2_get_drv_priv(vb->vb2_queue);567 unsigned int size =568 (vc->width * vc->height * vc->format->depth) >> 3;569 570 if (vb2_plane_size(vb, 0) < size)571 return -EINVAL;572 vb2_set_plane_payload(vb, 0, size);573 return 0;574}575 576static const struct vb2_ops tw686x_video_qops = {577 .queue_setup = tw686x_queue_setup,578 .buf_queue = tw686x_buf_queue,579 .buf_prepare = tw686x_buf_prepare,580 .start_streaming = tw686x_start_streaming,581 .stop_streaming = tw686x_stop_streaming,582 .wait_prepare = vb2_ops_wait_prepare,583 .wait_finish = vb2_ops_wait_finish,584};585 586static int tw686x_s_ctrl(struct v4l2_ctrl *ctrl)587{588 struct tw686x_video_channel *vc;589 struct tw686x_dev *dev;590 unsigned int ch;591 592 vc = container_of(ctrl->handler, struct tw686x_video_channel,593 ctrl_handler);594 dev = vc->dev;595 ch = vc->ch;596 597 switch (ctrl->id) {598 case V4L2_CID_BRIGHTNESS:599 reg_write(dev, BRIGHT[ch], ctrl->val & 0xff);600 return 0;601 602 case V4L2_CID_CONTRAST:603 reg_write(dev, CONTRAST[ch], ctrl->val);604 return 0;605 606 case V4L2_CID_SATURATION:607 reg_write(dev, SAT_U[ch], ctrl->val);608 reg_write(dev, SAT_V[ch], ctrl->val);609 return 0;610 611 case V4L2_CID_HUE:612 reg_write(dev, HUE[ch], ctrl->val & 0xff);613 return 0;614 }615 616 return -EINVAL;617}618 619static const struct v4l2_ctrl_ops ctrl_ops = {620 .s_ctrl = tw686x_s_ctrl,621};622 623static int tw686x_g_fmt_vid_cap(struct file *file, void *priv,624 struct v4l2_format *f)625{626 struct tw686x_video_channel *vc = video_drvdata(file);627 struct tw686x_dev *dev = vc->dev;628 629 f->fmt.pix.width = vc->width;630 f->fmt.pix.height = vc->height;631 f->fmt.pix.field = dev->dma_ops->field;632 f->fmt.pix.pixelformat = vc->format->fourcc;633 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;634 f->fmt.pix.bytesperline = (f->fmt.pix.width * vc->format->depth) / 8;635 f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;636 return 0;637}638 639static int tw686x_try_fmt_vid_cap(struct file *file, void *priv,640 struct v4l2_format *f)641{642 struct tw686x_video_channel *vc = video_drvdata(file);643 struct tw686x_dev *dev = vc->dev;644 unsigned int video_height = TW686X_VIDEO_HEIGHT(vc->video_standard);645 const struct tw686x_format *format;646 647 format = format_by_fourcc(f->fmt.pix.pixelformat);648 if (!format) {649 format = &formats[0];650 f->fmt.pix.pixelformat = format->fourcc;651 }652 653 if (f->fmt.pix.width <= TW686X_VIDEO_WIDTH / 2)654 f->fmt.pix.width = TW686X_VIDEO_WIDTH / 2;655 else656 f->fmt.pix.width = TW686X_VIDEO_WIDTH;657 658 if (f->fmt.pix.height <= video_height / 2)659 f->fmt.pix.height = video_height / 2;660 else661 f->fmt.pix.height = video_height;662 663 f->fmt.pix.bytesperline = (f->fmt.pix.width * format->depth) / 8;664 f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;665 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;666 f->fmt.pix.field = dev->dma_ops->field;667 668 return 0;669}670 671static int tw686x_set_format(struct tw686x_video_channel *vc,672 unsigned int pixelformat, unsigned int width,673 unsigned int height, bool realloc)674{675 struct tw686x_dev *dev = vc->dev;676 u32 val, dma_width, dma_height, dma_line_width;677 int err, pb;678 679 vc->format = format_by_fourcc(pixelformat);680 vc->width = width;681 vc->height = height;682 683 /* We need new DMA buffers if the framesize has changed */684 if (dev->dma_ops->alloc && realloc) {685 for (pb = 0; pb < 2; pb++)686 dev->dma_ops->free(vc, pb);687 688 for (pb = 0; pb < 2; pb++) {689 err = dev->dma_ops->alloc(vc, pb);690 if (err) {691 if (pb > 0)692 dev->dma_ops->free(vc, 0);693 return err;694 }695 }696 }697 698 val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);699 700 if (vc->width <= TW686X_VIDEO_WIDTH / 2)701 val |= BIT(23);702 else703 val &= ~BIT(23);704 705 if (vc->height <= TW686X_VIDEO_HEIGHT(vc->video_standard) / 2)706 val |= BIT(24);707 else708 val &= ~BIT(24);709 710 val &= ~0x7ffff;711 712 /* Program the DMA scatter-gather */713 if (dev->dma_mode == TW686X_DMA_MODE_SG) {714 u32 start_idx, end_idx;715 716 start_idx = is_second_gen(dev) ?717 0 : vc->ch * TW686X_MAX_SG_DESC_COUNT;718 end_idx = start_idx + TW686X_MAX_SG_DESC_COUNT - 1;719 720 val |= (end_idx << 10) | start_idx;721 }722 723 val &= ~(0x7 << 20);724 val |= vc->format->mode << 20;725 reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);726 727 /* Program the DMA frame size */728 dma_width = (vc->width * 2) & 0x7ff;729 dma_height = vc->height / 2;730 dma_line_width = (vc->width * 2) & 0x7ff;731 val = (dma_height << 22) | (dma_line_width << 11) | dma_width;732 reg_write(vc->dev, VDMA_WHP[vc->ch], val);733 return 0;734}735 736static int tw686x_s_fmt_vid_cap(struct file *file, void *priv,737 struct v4l2_format *f)738{739 struct tw686x_video_channel *vc = video_drvdata(file);740 unsigned long area;741 bool realloc;742 int err;743 744 if (vb2_is_busy(&vc->vidq))745 return -EBUSY;746 747 area = vc->width * vc->height;748 err = tw686x_try_fmt_vid_cap(file, priv, f);749 if (err)750 return err;751 752 realloc = area != (f->fmt.pix.width * f->fmt.pix.height);753 return tw686x_set_format(vc, f->fmt.pix.pixelformat,754 f->fmt.pix.width, f->fmt.pix.height,755 realloc);756}757 758static int tw686x_querycap(struct file *file, void *priv,759 struct v4l2_capability *cap)760{761 struct tw686x_video_channel *vc = video_drvdata(file);762 struct tw686x_dev *dev = vc->dev;763 764 strscpy(cap->driver, "tw686x", sizeof(cap->driver));765 strscpy(cap->card, dev->name, sizeof(cap->card));766 return 0;767}768 769static int tw686x_set_standard(struct tw686x_video_channel *vc, v4l2_std_id id)770{771 u32 val;772 773 if (id & V4L2_STD_NTSC)774 val = 0;775 else if (id & V4L2_STD_PAL)776 val = 1;777 else if (id & V4L2_STD_SECAM)778 val = 2;779 else if (id & V4L2_STD_NTSC_443)780 val = 3;781 else if (id & V4L2_STD_PAL_M)782 val = 4;783 else if (id & V4L2_STD_PAL_Nc)784 val = 5;785 else if (id & V4L2_STD_PAL_60)786 val = 6;787 else788 return -EINVAL;789 790 vc->video_standard = id;791 reg_write(vc->dev, SDT[vc->ch], val);792 793 val = reg_read(vc->dev, VIDEO_CONTROL1);794 if (id & V4L2_STD_525_60)795 val &= ~(1 << (SYS_MODE_DMA_SHIFT + vc->ch));796 else797 val |= (1 << (SYS_MODE_DMA_SHIFT + vc->ch));798 reg_write(vc->dev, VIDEO_CONTROL1, val);799 800 return 0;801}802 803static int tw686x_s_std(struct file *file, void *priv, v4l2_std_id id)804{805 struct tw686x_video_channel *vc = video_drvdata(file);806 struct v4l2_format f;807 int ret;808 809 if (vc->video_standard == id)810 return 0;811 812 if (vb2_is_busy(&vc->vidq))813 return -EBUSY;814 815 ret = tw686x_set_standard(vc, id);816 if (ret)817 return ret;818 /*819 * Adjust format after V4L2_STD_525_60/V4L2_STD_625_50 change,820 * calling g_fmt and s_fmt will sanitize the height821 * according to the standard.822 */823 tw686x_g_fmt_vid_cap(file, priv, &f);824 tw686x_s_fmt_vid_cap(file, priv, &f);825 826 /*827 * Frame decimation depends on the chosen standard,828 * so reset it to the current value.829 */830 tw686x_set_framerate(vc, vc->fps);831 return 0;832}833 834static int tw686x_querystd(struct file *file, void *priv, v4l2_std_id *std)835{836 struct tw686x_video_channel *vc = video_drvdata(file);837 struct tw686x_dev *dev = vc->dev;838 unsigned int old_std, detected_std = 0;839 unsigned long end;840 841 if (vb2_is_streaming(&vc->vidq))842 return -EBUSY;843 844 /* Enable and start standard detection */845 old_std = reg_read(dev, SDT[vc->ch]);846 reg_write(dev, SDT[vc->ch], 0x7);847 reg_write(dev, SDT_EN[vc->ch], 0xff);848 849 end = jiffies + msecs_to_jiffies(500);850 while (time_is_after_jiffies(end)) {851 852 detected_std = reg_read(dev, SDT[vc->ch]);853 if (!(detected_std & BIT(7)))854 break;855 msleep(100);856 }857 reg_write(dev, SDT[vc->ch], old_std);858 859 /* Exit if still busy */860 if (detected_std & BIT(7))861 return 0;862 863 detected_std = (detected_std >> 4) & 0x7;864 switch (detected_std) {865 case TW686X_STD_NTSC_M:866 *std &= V4L2_STD_NTSC;867 break;868 case TW686X_STD_NTSC_443:869 *std &= V4L2_STD_NTSC_443;870 break;871 case TW686X_STD_PAL_M:872 *std &= V4L2_STD_PAL_M;873 break;874 case TW686X_STD_PAL_60:875 *std &= V4L2_STD_PAL_60;876 break;877 case TW686X_STD_PAL:878 *std &= V4L2_STD_PAL;879 break;880 case TW686X_STD_PAL_CN:881 *std &= V4L2_STD_PAL_Nc;882 break;883 case TW686X_STD_SECAM:884 *std &= V4L2_STD_SECAM;885 break;886 default:887 *std = 0;888 }889 return 0;890}891 892static int tw686x_g_std(struct file *file, void *priv, v4l2_std_id *id)893{894 struct tw686x_video_channel *vc = video_drvdata(file);895 896 *id = vc->video_standard;897 return 0;898}899 900static int tw686x_enum_framesizes(struct file *file, void *priv,901 struct v4l2_frmsizeenum *fsize)902{903 struct tw686x_video_channel *vc = video_drvdata(file);904 905 if (fsize->index)906 return -EINVAL;907 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;908 fsize->stepwise.max_width = TW686X_VIDEO_WIDTH;909 fsize->stepwise.min_width = fsize->stepwise.max_width / 2;910 fsize->stepwise.step_width = fsize->stepwise.min_width;911 fsize->stepwise.max_height = TW686X_VIDEO_HEIGHT(vc->video_standard);912 fsize->stepwise.min_height = fsize->stepwise.max_height / 2;913 fsize->stepwise.step_height = fsize->stepwise.min_height;914 return 0;915}916 917static int tw686x_enum_frameintervals(struct file *file, void *priv,918 struct v4l2_frmivalenum *ival)919{920 struct tw686x_video_channel *vc = video_drvdata(file);921 int max_fps = TW686X_MAX_FPS(vc->video_standard);922 int max_rates = DIV_ROUND_UP(max_fps, 2);923 924 if (ival->index >= max_rates)925 return -EINVAL;926 927 ival->type = V4L2_FRMIVAL_TYPE_DISCRETE;928 ival->discrete.numerator = 1;929 if (ival->index < (max_rates - 1))930 ival->discrete.denominator = (ival->index + 1) * 2;931 else932 ival->discrete.denominator = max_fps;933 return 0;934}935 936static int tw686x_g_parm(struct file *file, void *priv,937 struct v4l2_streamparm *sp)938{939 struct tw686x_video_channel *vc = video_drvdata(file);940 struct v4l2_captureparm *cp = &sp->parm.capture;941 942 if (sp->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)943 return -EINVAL;944 sp->parm.capture.readbuffers = 3;945 946 cp->capability = V4L2_CAP_TIMEPERFRAME;947 cp->timeperframe.numerator = 1;948 cp->timeperframe.denominator = vc->fps;949 return 0;950}951 952static int tw686x_s_parm(struct file *file, void *priv,953 struct v4l2_streamparm *sp)954{955 struct tw686x_video_channel *vc = video_drvdata(file);956 struct v4l2_captureparm *cp = &sp->parm.capture;957 unsigned int denominator = cp->timeperframe.denominator;958 unsigned int numerator = cp->timeperframe.numerator;959 unsigned int fps;960 961 if (vb2_is_busy(&vc->vidq))962 return -EBUSY;963 964 fps = (!numerator || !denominator) ? 0 : denominator / numerator;965 if (vc->fps != fps)966 tw686x_set_framerate(vc, fps);967 return tw686x_g_parm(file, priv, sp);968}969 970static int tw686x_enum_fmt_vid_cap(struct file *file, void *priv,971 struct v4l2_fmtdesc *f)972{973 if (f->index >= ARRAY_SIZE(formats))974 return -EINVAL;975 f->pixelformat = formats[f->index].fourcc;976 return 0;977}978 979static void tw686x_set_input(struct tw686x_video_channel *vc, unsigned int i)980{981 u32 val;982 983 vc->input = i;984 985 val = reg_read(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch]);986 val &= ~(0x3 << 30);987 val |= i << 30;988 reg_write(vc->dev, VDMA_CHANNEL_CONFIG[vc->ch], val);989}990 991static int tw686x_s_input(struct file *file, void *priv, unsigned int i)992{993 struct tw686x_video_channel *vc = video_drvdata(file);994 995 if (i >= TW686X_INPUTS_PER_CH)996 return -EINVAL;997 if (i == vc->input)998 return 0;999 /*1000 * Not sure we are able to support on the fly input change1001 */1002 if (vb2_is_busy(&vc->vidq))1003 return -EBUSY;1004 1005 tw686x_set_input(vc, i);1006 return 0;1007}1008 1009static int tw686x_g_input(struct file *file, void *priv, unsigned int *i)1010{1011 struct tw686x_video_channel *vc = video_drvdata(file);1012 1013 *i = vc->input;1014 return 0;1015}1016 1017static int tw686x_enum_input(struct file *file, void *priv,1018 struct v4l2_input *i)1019{1020 struct tw686x_video_channel *vc = video_drvdata(file);1021 unsigned int vidstat;1022 1023 if (i->index >= TW686X_INPUTS_PER_CH)1024 return -EINVAL;1025 1026 snprintf(i->name, sizeof(i->name), "Composite%d", i->index);1027 i->type = V4L2_INPUT_TYPE_CAMERA;1028 i->std = vc->device->tvnorms;1029 i->capabilities = V4L2_IN_CAP_STD;1030 1031 vidstat = reg_read(vc->dev, VIDSTAT[vc->ch]);1032 i->status = 0;1033 if (vidstat & TW686X_VIDSTAT_VDLOSS)1034 i->status |= V4L2_IN_ST_NO_SIGNAL;1035 if (!(vidstat & TW686X_VIDSTAT_HLOCK))1036 i->status |= V4L2_IN_ST_NO_H_LOCK;1037 1038 return 0;1039}1040 1041static const struct v4l2_file_operations tw686x_video_fops = {1042 .owner = THIS_MODULE,1043 .open = v4l2_fh_open,1044 .unlocked_ioctl = video_ioctl2,1045 .release = vb2_fop_release,1046 .poll = vb2_fop_poll,1047 .read = vb2_fop_read,1048 .mmap = vb2_fop_mmap,1049};1050 1051static const struct v4l2_ioctl_ops tw686x_video_ioctl_ops = {1052 .vidioc_querycap = tw686x_querycap,1053 .vidioc_g_fmt_vid_cap = tw686x_g_fmt_vid_cap,1054 .vidioc_s_fmt_vid_cap = tw686x_s_fmt_vid_cap,1055 .vidioc_enum_fmt_vid_cap = tw686x_enum_fmt_vid_cap,1056 .vidioc_try_fmt_vid_cap = tw686x_try_fmt_vid_cap,1057 1058 .vidioc_querystd = tw686x_querystd,1059 .vidioc_g_std = tw686x_g_std,1060 .vidioc_s_std = tw686x_s_std,1061 1062 .vidioc_g_parm = tw686x_g_parm,1063 .vidioc_s_parm = tw686x_s_parm,1064 .vidioc_enum_framesizes = tw686x_enum_framesizes,1065 .vidioc_enum_frameintervals = tw686x_enum_frameintervals,1066 1067 .vidioc_enum_input = tw686x_enum_input,1068 .vidioc_g_input = tw686x_g_input,1069 .vidioc_s_input = tw686x_s_input,1070 1071 .vidioc_reqbufs = vb2_ioctl_reqbufs,1072 .vidioc_querybuf = vb2_ioctl_querybuf,1073 .vidioc_qbuf = vb2_ioctl_qbuf,1074 .vidioc_dqbuf = vb2_ioctl_dqbuf,1075 .vidioc_create_bufs = vb2_ioctl_create_bufs,1076 .vidioc_streamon = vb2_ioctl_streamon,1077 .vidioc_streamoff = vb2_ioctl_streamoff,1078 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,1079 1080 .vidioc_log_status = v4l2_ctrl_log_status,1081 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,1082 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,1083};1084 1085void tw686x_video_irq(struct tw686x_dev *dev, unsigned long requests,1086 unsigned int pb_status, unsigned int fifo_status,1087 unsigned int *reset_ch)1088{1089 struct tw686x_video_channel *vc;1090 unsigned long flags;1091 unsigned int ch, pb;1092 1093 for_each_set_bit(ch, &requests, max_channels(dev)) {1094 vc = &dev->video_channels[ch];1095 1096 /*1097 * This can either be a blue frame (with signal-lost bit set)1098 * or a good frame (with signal-lost bit clear). If we have just1099 * got signal, then this channel needs resetting.1100 */1101 if (vc->no_signal && !(fifo_status & BIT(ch))) {1102 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,1103 "video%d: signal recovered\n", vc->num);1104 vc->no_signal = false;1105 *reset_ch |= BIT(ch);1106 vc->pb = 0;1107 continue;1108 }1109 vc->no_signal = !!(fifo_status & BIT(ch));1110 1111 /* Check FIFO errors only if there's signal */1112 if (!vc->no_signal) {1113 u32 fifo_ov, fifo_bad;1114 1115 fifo_ov = (fifo_status >> 24) & BIT(ch);1116 fifo_bad = (fifo_status >> 16) & BIT(ch);1117 if (fifo_ov || fifo_bad) {1118 /* Mark this channel for reset */1119 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,1120 "video%d: FIFO error\n", vc->num);1121 *reset_ch |= BIT(ch);1122 vc->pb = 0;1123 continue;1124 }1125 }1126 1127 pb = !!(pb_status & BIT(ch));1128 if (vc->pb != pb) {1129 /* Mark this channel for reset */1130 v4l2_printk(KERN_DEBUG, &dev->v4l2_dev,1131 "video%d: unexpected p-b buffer!\n",1132 vc->num);1133 *reset_ch |= BIT(ch);1134 vc->pb = 0;1135 continue;1136 }1137 1138 spin_lock_irqsave(&vc->qlock, flags);1139 tw686x_buf_done(vc, pb);1140 dev->dma_ops->buf_refill(vc, pb);1141 spin_unlock_irqrestore(&vc->qlock, flags);1142 }1143}1144 1145void tw686x_video_free(struct tw686x_dev *dev)1146{1147 unsigned int ch, pb;1148 1149 for (ch = 0; ch < max_channels(dev); ch++) {1150 struct tw686x_video_channel *vc = &dev->video_channels[ch];1151 1152 video_unregister_device(vc->device);1153 1154 if (dev->dma_ops->free)1155 for (pb = 0; pb < 2; pb++)1156 dev->dma_ops->free(vc, pb);1157 }1158}1159 1160int tw686x_video_init(struct tw686x_dev *dev)1161{1162 unsigned int ch, val;1163 int err;1164 1165 if (dev->dma_mode == TW686X_DMA_MODE_MEMCPY)1166 dev->dma_ops = &memcpy_dma_ops;1167 else if (dev->dma_mode == TW686X_DMA_MODE_CONTIG)1168 dev->dma_ops = &contig_dma_ops;1169 else if (dev->dma_mode == TW686X_DMA_MODE_SG)1170 dev->dma_ops = &sg_dma_ops;1171 else1172 return -EINVAL;1173 1174 err = v4l2_device_register(&dev->pci_dev->dev, &dev->v4l2_dev);1175 if (err)1176 return err;1177 1178 if (dev->dma_ops->setup) {1179 err = dev->dma_ops->setup(dev);1180 if (err)1181 return err;1182 }1183 1184 /* Initialize vc->dev and vc->ch for the error path */1185 for (ch = 0; ch < max_channels(dev); ch++) {1186 struct tw686x_video_channel *vc = &dev->video_channels[ch];1187 1188 vc->dev = dev;1189 vc->ch = ch;1190 }1191 1192 for (ch = 0; ch < max_channels(dev); ch++) {1193 struct tw686x_video_channel *vc = &dev->video_channels[ch];1194 struct video_device *vdev;1195 1196 mutex_init(&vc->vb_mutex);1197 spin_lock_init(&vc->qlock);1198 INIT_LIST_HEAD(&vc->vidq_queued);1199 1200 /* default settings */1201 err = tw686x_set_standard(vc, V4L2_STD_NTSC);1202 if (err)1203 goto error;1204 1205 err = tw686x_set_format(vc, formats[0].fourcc,1206 TW686X_VIDEO_WIDTH,1207 TW686X_VIDEO_HEIGHT(vc->video_standard),1208 true);1209 if (err)1210 goto error;1211 1212 tw686x_set_input(vc, 0);1213 tw686x_set_framerate(vc, 30);1214 reg_write(dev, VDELAY_LO[ch], 0x14);1215 reg_write(dev, HACTIVE_LO[ch], 0xd0);1216 reg_write(dev, VIDEO_SIZE[ch], 0);1217 1218 vc->vidq.io_modes = VB2_READ | VB2_MMAP | VB2_DMABUF;1219 vc->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;1220 vc->vidq.drv_priv = vc;1221 vc->vidq.buf_struct_size = sizeof(struct tw686x_v4l2_buf);1222 vc->vidq.ops = &tw686x_video_qops;1223 vc->vidq.mem_ops = dev->dma_ops->mem_ops;1224 vc->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;1225 vc->vidq.min_queued_buffers = 2;1226 vc->vidq.lock = &vc->vb_mutex;1227 vc->vidq.gfp_flags = dev->dma_mode != TW686X_DMA_MODE_MEMCPY ?1228 GFP_DMA32 : 0;1229 vc->vidq.dev = &dev->pci_dev->dev;1230 1231 err = vb2_queue_init(&vc->vidq);1232 if (err) {1233 v4l2_err(&dev->v4l2_dev,1234 "dma%d: cannot init vb2 queue\n", ch);1235 goto error;1236 }1237 1238 err = v4l2_ctrl_handler_init(&vc->ctrl_handler, 4);1239 if (err) {1240 v4l2_err(&dev->v4l2_dev,1241 "dma%d: cannot init ctrl handler\n", ch);1242 goto error;1243 }1244 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,1245 V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);1246 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,1247 V4L2_CID_CONTRAST, 0, 255, 1, 100);1248 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,1249 V4L2_CID_SATURATION, 0, 255, 1, 128);1250 v4l2_ctrl_new_std(&vc->ctrl_handler, &ctrl_ops,1251 V4L2_CID_HUE, -128, 127, 1, 0);1252 err = vc->ctrl_handler.error;1253 if (err)1254 goto error;1255 1256 err = v4l2_ctrl_handler_setup(&vc->ctrl_handler);1257 if (err)1258 goto error;1259 1260 vdev = video_device_alloc();1261 if (!vdev) {1262 v4l2_err(&dev->v4l2_dev,1263 "dma%d: unable to allocate device\n", ch);1264 err = -ENOMEM;1265 goto error;1266 }1267 1268 snprintf(vdev->name, sizeof(vdev->name), "%s video", dev->name);1269 vdev->fops = &tw686x_video_fops;1270 vdev->ioctl_ops = &tw686x_video_ioctl_ops;1271 vdev->release = video_device_release;1272 vdev->v4l2_dev = &dev->v4l2_dev;1273 vdev->queue = &vc->vidq;1274 vdev->tvnorms = V4L2_STD_525_60 | V4L2_STD_625_50;1275 vdev->minor = -1;1276 vdev->lock = &vc->vb_mutex;1277 vdev->ctrl_handler = &vc->ctrl_handler;1278 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE |1279 V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;1280 vc->device = vdev;1281 video_set_drvdata(vdev, vc);1282 1283 err = video_register_device(vdev, VFL_TYPE_VIDEO, -1);1284 if (err < 0) {1285 video_device_release(vdev);1286 goto error;1287 }1288 vc->num = vdev->num;1289 }1290 1291 val = TW686X_DEF_PHASE_REF;1292 for (ch = 0; ch < max_channels(dev); ch++)1293 val |= dev->dma_ops->hw_dma_mode << (16 + ch * 2);1294 reg_write(dev, PHASE_REF, val);1295 1296 reg_write(dev, MISC2[0], 0xe7);1297 reg_write(dev, VCTRL1[0], 0xcc);1298 reg_write(dev, LOOP[0], 0xa5);1299 if (max_channels(dev) > 4) {1300 reg_write(dev, VCTRL1[1], 0xcc);1301 reg_write(dev, LOOP[1], 0xa5);1302 reg_write(dev, MISC2[1], 0xe7);1303 }1304 return 0;1305 1306error:1307 tw686x_video_free(dev);1308 return err;1309}1310