linux/drivers/media/pci/tw5864/tw5864-video.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0-or-later
   2/*
   3 *  TW5864 driver - video encoding functions
   4 *
   5 *  Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.com>
   6 */
   7
   8#include <linux/module.h>
   9#include <media/v4l2-common.h>
  10#include <media/v4l2-event.h>
  11#include <media/videobuf2-dma-contig.h>
  12
  13#include "tw5864.h"
  14#include "tw5864-reg.h"
  15
  16#define QUANTIZATION_TABLE_LEN 96
  17#define VLC_LOOKUP_TABLE_LEN 1024
  18
  19static const u16 forward_quantization_table[QUANTIZATION_TABLE_LEN] = {
  20        0x3333, 0x1f82, 0x3333, 0x1f82, 0x1f82, 0x147b, 0x1f82, 0x147b,
  21        0x3333, 0x1f82, 0x3333, 0x1f82, 0x1f82, 0x147b, 0x1f82, 0x147b,
  22        0x2e8c, 0x1d42, 0x2e8c, 0x1d42, 0x1d42, 0x1234, 0x1d42, 0x1234,
  23        0x2e8c, 0x1d42, 0x2e8c, 0x1d42, 0x1d42, 0x1234, 0x1d42, 0x1234,
  24        0x2762, 0x199a, 0x2762, 0x199a, 0x199a, 0x1062, 0x199a, 0x1062,
  25        0x2762, 0x199a, 0x2762, 0x199a, 0x199a, 0x1062, 0x199a, 0x1062,
  26        0x2492, 0x16c1, 0x2492, 0x16c1, 0x16c1, 0x0e3f, 0x16c1, 0x0e3f,
  27        0x2492, 0x16c1, 0x2492, 0x16c1, 0x16c1, 0x0e3f, 0x16c1, 0x0e3f,
  28        0x2000, 0x147b, 0x2000, 0x147b, 0x147b, 0x0d1b, 0x147b, 0x0d1b,
  29        0x2000, 0x147b, 0x2000, 0x147b, 0x147b, 0x0d1b, 0x147b, 0x0d1b,
  30        0x1c72, 0x11cf, 0x1c72, 0x11cf, 0x11cf, 0x0b4d, 0x11cf, 0x0b4d,
  31        0x1c72, 0x11cf, 0x1c72, 0x11cf, 0x11cf, 0x0b4d, 0x11cf, 0x0b4d
  32};
  33
  34static const u16 inverse_quantization_table[QUANTIZATION_TABLE_LEN] = {
  35        0x800a, 0x800d, 0x800a, 0x800d, 0x800d, 0x8010, 0x800d, 0x8010,
  36        0x800a, 0x800d, 0x800a, 0x800d, 0x800d, 0x8010, 0x800d, 0x8010,
  37        0x800b, 0x800e, 0x800b, 0x800e, 0x800e, 0x8012, 0x800e, 0x8012,
  38        0x800b, 0x800e, 0x800b, 0x800e, 0x800e, 0x8012, 0x800e, 0x8012,
  39        0x800d, 0x8010, 0x800d, 0x8010, 0x8010, 0x8014, 0x8010, 0x8014,
  40        0x800d, 0x8010, 0x800d, 0x8010, 0x8010, 0x8014, 0x8010, 0x8014,
  41        0x800e, 0x8012, 0x800e, 0x8012, 0x8012, 0x8017, 0x8012, 0x8017,
  42        0x800e, 0x8012, 0x800e, 0x8012, 0x8012, 0x8017, 0x8012, 0x8017,
  43        0x8010, 0x8014, 0x8010, 0x8014, 0x8014, 0x8019, 0x8014, 0x8019,
  44        0x8010, 0x8014, 0x8010, 0x8014, 0x8014, 0x8019, 0x8014, 0x8019,
  45        0x8012, 0x8017, 0x8012, 0x8017, 0x8017, 0x801d, 0x8017, 0x801d,
  46        0x8012, 0x8017, 0x8012, 0x8017, 0x8017, 0x801d, 0x8017, 0x801d
  47};
  48
  49static const u16 encoder_vlc_lookup_table[VLC_LOOKUP_TABLE_LEN] = {
  50        0x011, 0x000, 0x000, 0x000, 0x065, 0x021, 0x000, 0x000, 0x087, 0x064,
  51        0x031, 0x000, 0x097, 0x086, 0x075, 0x053, 0x0a7, 0x096, 0x085, 0x063,
  52        0x0b7, 0x0a6, 0x095, 0x074, 0x0df, 0x0b6, 0x0a5, 0x084, 0x0db, 0x0de,
  53        0x0b5, 0x094, 0x0d8, 0x0da, 0x0dd, 0x0a4, 0x0ef, 0x0ee, 0x0d9, 0x0b4,
  54        0x0eb, 0x0ea, 0x0ed, 0x0dc, 0x0ff, 0x0fe, 0x0e9, 0x0ec, 0x0fb, 0x0fa,
  55        0x0fd, 0x0e8, 0x10f, 0x0f1, 0x0f9, 0x0fc, 0x10b, 0x10e, 0x10d, 0x0f8,
  56        0x107, 0x10a, 0x109, 0x10c, 0x104, 0x106, 0x105, 0x108, 0x023, 0x000,
  57        0x000, 0x000, 0x06b, 0x022, 0x000, 0x000, 0x067, 0x057, 0x033, 0x000,
  58        0x077, 0x06a, 0x069, 0x045, 0x087, 0x066, 0x065, 0x044, 0x084, 0x076,
  59        0x075, 0x056, 0x097, 0x086, 0x085, 0x068, 0x0bf, 0x096, 0x095, 0x064,
  60        0x0bb, 0x0be, 0x0bd, 0x074, 0x0cf, 0x0ba, 0x0b9, 0x094, 0x0cb, 0x0ce,
  61        0x0cd, 0x0bc, 0x0c8, 0x0ca, 0x0c9, 0x0b8, 0x0df, 0x0de, 0x0dd, 0x0cc,
  62        0x0db, 0x0da, 0x0d9, 0x0dc, 0x0d7, 0x0eb, 0x0d6, 0x0d8, 0x0e9, 0x0e8,
  63        0x0ea, 0x0d1, 0x0e7, 0x0e6, 0x0e5, 0x0e4, 0x04f, 0x000, 0x000, 0x000,
  64        0x06f, 0x04e, 0x000, 0x000, 0x06b, 0x05f, 0x04d, 0x000, 0x068, 0x05c,
  65        0x05e, 0x04c, 0x07f, 0x05a, 0x05b, 0x04b, 0x07b, 0x058, 0x059, 0x04a,
  66        0x079, 0x06e, 0x06d, 0x049, 0x078, 0x06a, 0x069, 0x048, 0x08f, 0x07e,
  67        0x07d, 0x05d, 0x08b, 0x08e, 0x07a, 0x06c, 0x09f, 0x08a, 0x08d, 0x07c,
  68        0x09b, 0x09e, 0x089, 0x08c, 0x098, 0x09a, 0x09d, 0x088, 0x0ad, 0x097,
  69        0x099, 0x09c, 0x0a9, 0x0ac, 0x0ab, 0x0aa, 0x0a5, 0x0a8, 0x0a7, 0x0a6,
  70        0x0a1, 0x0a4, 0x0a3, 0x0a2, 0x021, 0x000, 0x000, 0x000, 0x067, 0x011,
  71        0x000, 0x000, 0x064, 0x066, 0x031, 0x000, 0x063, 0x073, 0x072, 0x065,
  72        0x062, 0x083, 0x082, 0x070, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  73        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  74        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  75        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  76        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  77        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  78        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  79        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  80        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  81        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  82        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  83        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  84        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  85        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  86        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  87        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  88        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  89        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  90        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  91        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  92        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  93        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  94        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  95        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  96        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  97        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  98        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
  99        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 100        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 101        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 102        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x011, 0x010,
 103        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 104        0x000, 0x000, 0x000, 0x000, 0x011, 0x021, 0x020, 0x000, 0x000, 0x000,
 105        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 106        0x023, 0x022, 0x021, 0x020, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 107        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x023, 0x022, 0x021, 0x031,
 108        0x030, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 109        0x000, 0x000, 0x023, 0x022, 0x033, 0x032, 0x031, 0x030, 0x000, 0x000,
 110        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x023, 0x030,
 111        0x031, 0x033, 0x032, 0x035, 0x034, 0x000, 0x000, 0x000, 0x000, 0x000,
 112        0x000, 0x000, 0x000, 0x000, 0x037, 0x036, 0x035, 0x034, 0x033, 0x032,
 113        0x031, 0x041, 0x051, 0x061, 0x071, 0x081, 0x091, 0x0a1, 0x0b1, 0x000,
 114        0x002, 0x000, 0x0e4, 0x011, 0x0f4, 0x002, 0x024, 0x003, 0x005, 0x012,
 115        0x034, 0x013, 0x065, 0x024, 0x013, 0x063, 0x015, 0x022, 0x075, 0x034,
 116        0x044, 0x023, 0x023, 0x073, 0x054, 0x033, 0x033, 0x004, 0x043, 0x014,
 117        0x011, 0x043, 0x014, 0x001, 0x025, 0x015, 0x035, 0x025, 0x064, 0x055,
 118        0x045, 0x035, 0x074, 0x065, 0x085, 0x0d5, 0x012, 0x095, 0x055, 0x045,
 119        0x095, 0x0e5, 0x084, 0x075, 0x022, 0x0a5, 0x094, 0x085, 0x032, 0x0b5,
 120        0x003, 0x0c5, 0x001, 0x044, 0x0a5, 0x032, 0x0b5, 0x094, 0x0c5, 0x0a4,
 121        0x0a4, 0x054, 0x0d5, 0x0b4, 0x0b4, 0x064, 0x0f5, 0x0f5, 0x053, 0x0d4,
 122        0x0e5, 0x0c4, 0x105, 0x105, 0x0c4, 0x074, 0x063, 0x0e4, 0x0d4, 0x084,
 123        0x073, 0x0f4, 0x004, 0x005, 0x000, 0x053, 0x000, 0x000, 0x000, 0x000,
 124        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 125        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 126        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 127        0x000, 0x000, 0x011, 0x021, 0x031, 0x030, 0x011, 0x021, 0x020, 0x000,
 128        0x011, 0x010, 0x000, 0x000, 0x011, 0x033, 0x032, 0x043, 0x042, 0x053,
 129        0x052, 0x063, 0x062, 0x073, 0x072, 0x083, 0x082, 0x093, 0x092, 0x091,
 130        0x037, 0x036, 0x035, 0x034, 0x033, 0x045, 0x044, 0x043, 0x042, 0x053,
 131        0x052, 0x063, 0x062, 0x061, 0x060, 0x000, 0x045, 0x037, 0x036, 0x035,
 132        0x044, 0x043, 0x034, 0x033, 0x042, 0x053, 0x052, 0x061, 0x051, 0x060,
 133        0x000, 0x000, 0x053, 0x037, 0x045, 0x044, 0x036, 0x035, 0x034, 0x043,
 134        0x033, 0x042, 0x052, 0x051, 0x050, 0x000, 0x000, 0x000, 0x045, 0x044,
 135        0x043, 0x037, 0x036, 0x035, 0x034, 0x033, 0x042, 0x051, 0x041, 0x050,
 136        0x000, 0x000, 0x000, 0x000, 0x061, 0x051, 0x037, 0x036, 0x035, 0x034,
 137        0x033, 0x032, 0x041, 0x031, 0x060, 0x000, 0x000, 0x000, 0x000, 0x000,
 138        0x061, 0x051, 0x035, 0x034, 0x033, 0x023, 0x032, 0x041, 0x031, 0x060,
 139        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x061, 0x041, 0x051, 0x033,
 140        0x023, 0x022, 0x032, 0x031, 0x060, 0x000, 0x000, 0x000, 0x000, 0x000,
 141        0x000, 0x000, 0x061, 0x060, 0x041, 0x023, 0x022, 0x031, 0x021, 0x051,
 142        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x051, 0x050,
 143        0x031, 0x023, 0x022, 0x021, 0x041, 0x000, 0x000, 0x000, 0x000, 0x000,
 144        0x000, 0x000, 0x000, 0x000, 0x040, 0x041, 0x031, 0x032, 0x011, 0x033,
 145        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 146        0x040, 0x041, 0x021, 0x011, 0x031, 0x000, 0x000, 0x000, 0x000, 0x000,
 147        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x030, 0x031, 0x011, 0x021,
 148        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 149        0x000, 0x000, 0x020, 0x021, 0x011, 0x000, 0x000, 0x000, 0x000, 0x000,
 150        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x010, 0x011,
 151        0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000, 0x000,
 152        0x000, 0x000, 0x000, 0x000
 153};
 154
 155static const unsigned int lambda_lookup_table[] = {
 156        0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020,
 157        0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020, 0x0020,
 158        0x0040, 0x0040, 0x0040, 0x0040, 0x0060, 0x0060, 0x0060, 0x0080,
 159        0x0080, 0x0080, 0x00a0, 0x00c0, 0x00c0, 0x00e0, 0x0100, 0x0120,
 160        0x0140, 0x0160, 0x01a0, 0x01c0, 0x0200, 0x0240, 0x0280, 0x02e0,
 161        0x0320, 0x03a0, 0x0400, 0x0480, 0x0500, 0x05a0, 0x0660, 0x0720,
 162        0x0800, 0x0900, 0x0a20, 0x0b60
 163};
 164
 165static const unsigned int intra4x4_lambda3[] = {
 166        1, 1, 1, 1, 1, 1, 1, 1,
 167        1, 1, 1, 1, 1, 1, 1, 1,
 168        2, 2, 2, 2, 3, 3, 3, 4,
 169        4, 4, 5, 6, 6, 7, 8, 9,
 170        10, 11, 13, 14, 16, 18, 20, 23,
 171        25, 29, 32, 36, 40, 45, 51, 57,
 172        64, 72, 81, 91
 173};
 174
 175static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std std);
 176static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std);
 177
 178static void tw5864_handle_frame_task(struct tasklet_struct *t);
 179static void tw5864_handle_frame(struct tw5864_h264_frame *frame);
 180static void tw5864_frame_interval_set(struct tw5864_input *input);
 181
 182static int tw5864_queue_setup(struct vb2_queue *q, unsigned int *num_buffers,
 183                              unsigned int *num_planes, unsigned int sizes[],
 184                              struct device *alloc_ctxs[])
 185{
 186        if (*num_planes)
 187                return sizes[0] < H264_VLC_BUF_SIZE ? -EINVAL : 0;
 188
 189        sizes[0] = H264_VLC_BUF_SIZE;
 190        *num_planes = 1;
 191
 192        return 0;
 193}
 194
 195static void tw5864_buf_queue(struct vb2_buffer *vb)
 196{
 197        struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 198        struct vb2_queue *vq = vb->vb2_queue;
 199        struct tw5864_input *dev = vb2_get_drv_priv(vq);
 200        struct tw5864_buf *buf = container_of(vbuf, struct tw5864_buf, vb);
 201        unsigned long flags;
 202
 203        spin_lock_irqsave(&dev->slock, flags);
 204        list_add_tail(&buf->list, &dev->active);
 205        spin_unlock_irqrestore(&dev->slock, flags);
 206}
 207
 208static int tw5864_input_std_get(struct tw5864_input *input,
 209                                enum tw5864_vid_std *std)
 210{
 211        struct tw5864_dev *dev = input->root;
 212        u8 std_reg = tw_indir_readb(TW5864_INDIR_VIN_E(input->nr));
 213
 214        *std = (std_reg & 0x70) >> 4;
 215
 216        if (std_reg & 0x80) {
 217                dev_dbg(&dev->pci->dev,
 218                        "Video format detection is in progress, please wait\n");
 219                return -EAGAIN;
 220        }
 221
 222        return 0;
 223}
 224
 225static int tw5864_enable_input(struct tw5864_input *input)
 226{
 227        struct tw5864_dev *dev = input->root;
 228        int nr = input->nr;
 229        unsigned long flags;
 230        int d1_width = 720;
 231        int d1_height;
 232        int frame_width_bus_value = 0;
 233        int frame_height_bus_value = 0;
 234        int reg_frame_bus = 0x1c;
 235        int fmt_reg_value = 0;
 236        int downscale_enabled = 0;
 237
 238        dev_dbg(&dev->pci->dev, "Enabling channel %d\n", nr);
 239
 240        input->frame_seqno = 0;
 241        input->frame_gop_seqno = 0;
 242        input->h264_idr_pic_id = 0;
 243
 244        input->reg_dsp_qp = input->qp;
 245        input->reg_dsp_ref_mvp_lambda = lambda_lookup_table[input->qp];
 246        input->reg_dsp_i4x4_weight = intra4x4_lambda3[input->qp];
 247        input->reg_emu = TW5864_EMU_EN_LPF | TW5864_EMU_EN_BHOST
 248                | TW5864_EMU_EN_SEN | TW5864_EMU_EN_ME | TW5864_EMU_EN_DDR;
 249        input->reg_dsp = nr /* channel id */
 250                | TW5864_DSP_CHROM_SW
 251                | ((0xa << 8) & TW5864_DSP_MB_DELAY)
 252                ;
 253
 254        input->resolution = D1;
 255
 256        d1_height = (input->std == STD_NTSC) ? 480 : 576;
 257
 258        input->width = d1_width;
 259        input->height = d1_height;
 260
 261        input->reg_interlacing = 0x4;
 262
 263        switch (input->resolution) {
 264        case D1:
 265                frame_width_bus_value = 0x2cf;
 266                frame_height_bus_value = input->height - 1;
 267                reg_frame_bus = 0x1c;
 268                fmt_reg_value = 0;
 269                downscale_enabled = 0;
 270                input->reg_dsp_codec |= TW5864_CIF_MAP_MD | TW5864_HD1_MAP_MD;
 271                input->reg_emu |= TW5864_DSP_FRAME_TYPE_D1;
 272                input->reg_interlacing = TW5864_DI_EN | TW5864_DSP_INTER_ST;
 273
 274                tw_setl(TW5864_FULL_HALF_FLAG, 1 << nr);
 275                break;
 276        case HD1:
 277                input->height /= 2;
 278                input->width /= 2;
 279                frame_width_bus_value = 0x2cf;
 280                frame_height_bus_value = input->height * 2 - 1;
 281                reg_frame_bus = 0x1c;
 282                fmt_reg_value = 0;
 283                downscale_enabled = 0;
 284                input->reg_dsp_codec |= TW5864_HD1_MAP_MD;
 285                input->reg_emu |= TW5864_DSP_FRAME_TYPE_D1;
 286
 287                tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
 288
 289                break;
 290        case CIF:
 291                input->height /= 4;
 292                input->width /= 2;
 293                frame_width_bus_value = 0x15f;
 294                frame_height_bus_value = input->height * 2 - 1;
 295                reg_frame_bus = 0x07;
 296                fmt_reg_value = 1;
 297                downscale_enabled = 1;
 298                input->reg_dsp_codec |= TW5864_CIF_MAP_MD;
 299
 300                tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
 301                break;
 302        case QCIF:
 303                input->height /= 4;
 304                input->width /= 4;
 305                frame_width_bus_value = 0x15f;
 306                frame_height_bus_value = input->height * 2 - 1;
 307                reg_frame_bus = 0x07;
 308                fmt_reg_value = 1;
 309                downscale_enabled = 1;
 310                input->reg_dsp_codec |= TW5864_CIF_MAP_MD;
 311
 312                tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
 313                break;
 314        }
 315
 316        /* analog input width / 4 */
 317        tw_indir_writeb(TW5864_INDIR_IN_PIC_WIDTH(nr), d1_width / 4);
 318        tw_indir_writeb(TW5864_INDIR_IN_PIC_HEIGHT(nr), d1_height / 4);
 319
 320        /* output width / 4 */
 321        tw_indir_writeb(TW5864_INDIR_OUT_PIC_WIDTH(nr), input->width / 4);
 322        tw_indir_writeb(TW5864_INDIR_OUT_PIC_HEIGHT(nr), input->height / 4);
 323
 324        /*
 325         * Crop width from 720 to 704.
 326         * Above register settings need value 720 involved.
 327         */
 328        input->width = 704;
 329        tw_indir_writeb(TW5864_INDIR_CROP_ETC,
 330                        tw_indir_readb(TW5864_INDIR_CROP_ETC) |
 331                        TW5864_INDIR_CROP_ETC_CROP_EN);
 332
 333        tw_writel(TW5864_DSP_PIC_MAX_MB,
 334                  ((input->width / 16) << 8) | (input->height / 16));
 335
 336        tw_writel(TW5864_FRAME_WIDTH_BUS_A(nr),
 337                  frame_width_bus_value);
 338        tw_writel(TW5864_FRAME_WIDTH_BUS_B(nr),
 339                  frame_width_bus_value);
 340        tw_writel(TW5864_FRAME_HEIGHT_BUS_A(nr),
 341                  frame_height_bus_value);
 342        tw_writel(TW5864_FRAME_HEIGHT_BUS_B(nr),
 343                  (frame_height_bus_value + 1) / 2 - 1);
 344
 345        tw5864_frame_interval_set(input);
 346
 347        if (downscale_enabled)
 348                tw_setl(TW5864_H264EN_CH_DNS, 1 << nr);
 349
 350        tw_mask_shift_writel(TW5864_H264EN_CH_FMT_REG1, 0x3, 2 * nr,
 351                             fmt_reg_value);
 352
 353        tw_mask_shift_writel((nr < 2
 354                              ? TW5864_H264EN_RATE_MAX_LINE_REG1
 355                              : TW5864_H264EN_RATE_MAX_LINE_REG2),
 356                             0x1f, 5 * (nr % 2),
 357                             input->std == STD_NTSC ? 29 : 24);
 358
 359        tw_mask_shift_writel((nr < 2) ? TW5864_FRAME_BUS1 :
 360                             TW5864_FRAME_BUS2, 0xff, (nr % 2) * 8,
 361                             reg_frame_bus);
 362
 363        spin_lock_irqsave(&dev->slock, flags);
 364        input->enabled = 1;
 365        spin_unlock_irqrestore(&dev->slock, flags);
 366
 367        return 0;
 368}
 369
 370void tw5864_request_encoded_frame(struct tw5864_input *input)
 371{
 372        struct tw5864_dev *dev = input->root;
 373        u32 enc_buf_id_new;
 374
 375        tw_setl(TW5864_DSP_CODEC, TW5864_CIF_MAP_MD | TW5864_HD1_MAP_MD);
 376        tw_writel(TW5864_EMU, input->reg_emu);
 377        tw_writel(TW5864_INTERLACING, input->reg_interlacing);
 378        tw_writel(TW5864_DSP, input->reg_dsp);
 379
 380        tw_writel(TW5864_DSP_QP, input->reg_dsp_qp);
 381        tw_writel(TW5864_DSP_REF_MVP_LAMBDA, input->reg_dsp_ref_mvp_lambda);
 382        tw_writel(TW5864_DSP_I4x4_WEIGHT, input->reg_dsp_i4x4_weight);
 383        tw_mask_shift_writel(TW5864_DSP_INTRA_MODE, TW5864_DSP_INTRA_MODE_MASK,
 384                             TW5864_DSP_INTRA_MODE_SHIFT,
 385                             TW5864_DSP_INTRA_MODE_16x16);
 386
 387        if (input->frame_gop_seqno == 0) {
 388                /* Produce I-frame */
 389                tw_writel(TW5864_MOTION_SEARCH_ETC, TW5864_INTRA_EN);
 390                input->h264_idr_pic_id++;
 391                input->h264_idr_pic_id &= TW5864_DSP_REF_FRM;
 392        } else {
 393                /* Produce P-frame */
 394                tw_writel(TW5864_MOTION_SEARCH_ETC, TW5864_INTRA_EN |
 395                          TW5864_ME_EN | BIT(5) /* SRCH_OPT default */);
 396        }
 397        tw5864_prepare_frame_headers(input);
 398        tw_writel(TW5864_VLC,
 399                  TW5864_VLC_PCI_SEL |
 400                  ((input->tail_nb_bits + 24) << TW5864_VLC_BIT_ALIGN_SHIFT) |
 401                  input->reg_dsp_qp);
 402
 403        enc_buf_id_new = tw_mask_shift_readl(TW5864_ENC_BUF_PTR_REC1, 0x3,
 404                                             2 * input->nr);
 405        tw_writel(TW5864_DSP_ENC_ORG_PTR_REG,
 406                  enc_buf_id_new << TW5864_DSP_ENC_ORG_PTR_SHIFT);
 407        tw_writel(TW5864_DSP_ENC_REC,
 408                  enc_buf_id_new << 12 | ((enc_buf_id_new + 3) & 3));
 409
 410        tw_writel(TW5864_SLICE, TW5864_START_NSLICE);
 411        tw_writel(TW5864_SLICE, 0);
 412}
 413
 414static int tw5864_disable_input(struct tw5864_input *input)
 415{
 416        struct tw5864_dev *dev = input->root;
 417        unsigned long flags;
 418
 419        dev_dbg(&dev->pci->dev, "Disabling channel %d\n", input->nr);
 420
 421        spin_lock_irqsave(&dev->slock, flags);
 422        input->enabled = 0;
 423        spin_unlock_irqrestore(&dev->slock, flags);
 424        return 0;
 425}
 426
 427static int tw5864_start_streaming(struct vb2_queue *q, unsigned int count)
 428{
 429        struct tw5864_input *input = vb2_get_drv_priv(q);
 430        int ret;
 431
 432        ret = tw5864_enable_input(input);
 433        if (!ret)
 434                return 0;
 435
 436        while (!list_empty(&input->active)) {
 437                struct tw5864_buf *buf = list_entry(input->active.next,
 438                                                    struct tw5864_buf, list);
 439
 440                list_del(&buf->list);
 441                vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 442        }
 443        return ret;
 444}
 445
 446static void tw5864_stop_streaming(struct vb2_queue *q)
 447{
 448        unsigned long flags;
 449        struct tw5864_input *input = vb2_get_drv_priv(q);
 450
 451        tw5864_disable_input(input);
 452
 453        spin_lock_irqsave(&input->slock, flags);
 454        if (input->vb) {
 455                vb2_buffer_done(&input->vb->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 456                input->vb = NULL;
 457        }
 458        while (!list_empty(&input->active)) {
 459                struct tw5864_buf *buf = list_entry(input->active.next,
 460                                                    struct tw5864_buf, list);
 461
 462                list_del(&buf->list);
 463                vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 464        }
 465        spin_unlock_irqrestore(&input->slock, flags);
 466}
 467
 468static const struct vb2_ops tw5864_video_qops = {
 469        .queue_setup = tw5864_queue_setup,
 470        .buf_queue = tw5864_buf_queue,
 471        .start_streaming = tw5864_start_streaming,
 472        .stop_streaming = tw5864_stop_streaming,
 473        .wait_prepare = vb2_ops_wait_prepare,
 474        .wait_finish = vb2_ops_wait_finish,
 475};
 476
 477static int tw5864_s_ctrl(struct v4l2_ctrl *ctrl)
 478{
 479        struct tw5864_input *input =
 480                container_of(ctrl->handler, struct tw5864_input, hdl);
 481        struct tw5864_dev *dev = input->root;
 482        unsigned long flags;
 483
 484        switch (ctrl->id) {
 485        case V4L2_CID_BRIGHTNESS:
 486                tw_indir_writeb(TW5864_INDIR_VIN_A_BRIGHT(input->nr),
 487                                (u8)ctrl->val);
 488                break;
 489        case V4L2_CID_HUE:
 490                tw_indir_writeb(TW5864_INDIR_VIN_7_HUE(input->nr),
 491                                (u8)ctrl->val);
 492                break;
 493        case V4L2_CID_CONTRAST:
 494                tw_indir_writeb(TW5864_INDIR_VIN_9_CNTRST(input->nr),
 495                                (u8)ctrl->val);
 496                break;
 497        case V4L2_CID_SATURATION:
 498                tw_indir_writeb(TW5864_INDIR_VIN_B_SAT_U(input->nr),
 499                                (u8)ctrl->val);
 500                tw_indir_writeb(TW5864_INDIR_VIN_C_SAT_V(input->nr),
 501                                (u8)ctrl->val);
 502                break;
 503        case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
 504                input->gop = ctrl->val;
 505                return 0;
 506        case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
 507                spin_lock_irqsave(&input->slock, flags);
 508                input->qp = ctrl->val;
 509                input->reg_dsp_qp = input->qp;
 510                input->reg_dsp_ref_mvp_lambda = lambda_lookup_table[input->qp];
 511                input->reg_dsp_i4x4_weight = intra4x4_lambda3[input->qp];
 512                spin_unlock_irqrestore(&input->slock, flags);
 513                return 0;
 514        case V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD:
 515                memset(input->md_threshold_grid_values, ctrl->val,
 516                       sizeof(input->md_threshold_grid_values));
 517                return 0;
 518        case V4L2_CID_DETECT_MD_MODE:
 519                return 0;
 520        case V4L2_CID_DETECT_MD_THRESHOLD_GRID:
 521                /* input->md_threshold_grid_ctrl->p_new.p_u16 contains data */
 522                memcpy(input->md_threshold_grid_values,
 523                       input->md_threshold_grid_ctrl->p_new.p_u16,
 524                       sizeof(input->md_threshold_grid_values));
 525                return 0;
 526        }
 527        return 0;
 528}
 529
 530static int tw5864_fmt_vid_cap(struct file *file, void *priv,
 531                              struct v4l2_format *f)
 532{
 533        struct tw5864_input *input = video_drvdata(file);
 534
 535        f->fmt.pix.width = 704;
 536        switch (input->std) {
 537        default:
 538                WARN_ON_ONCE(1);
 539                return -EINVAL;
 540        case STD_NTSC:
 541                f->fmt.pix.height = 480;
 542                break;
 543        case STD_PAL:
 544        case STD_SECAM:
 545                f->fmt.pix.height = 576;
 546                break;
 547        }
 548        f->fmt.pix.field = V4L2_FIELD_INTERLACED;
 549        f->fmt.pix.pixelformat = V4L2_PIX_FMT_H264;
 550        f->fmt.pix.sizeimage = H264_VLC_BUF_SIZE;
 551        f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 552        return 0;
 553}
 554
 555static int tw5864_enum_input(struct file *file, void *priv,
 556                             struct v4l2_input *i)
 557{
 558        struct tw5864_input *input = video_drvdata(file);
 559        struct tw5864_dev *dev = input->root;
 560
 561        u8 indir_0x000 = tw_indir_readb(TW5864_INDIR_VIN_0(input->nr));
 562        u8 indir_0x00d = tw_indir_readb(TW5864_INDIR_VIN_D(input->nr));
 563        u8 v1 = indir_0x000;
 564        u8 v2 = indir_0x00d;
 565
 566        if (i->index)
 567                return -EINVAL;
 568
 569        i->type = V4L2_INPUT_TYPE_CAMERA;
 570        snprintf(i->name, sizeof(i->name), "Encoder %d", input->nr);
 571        i->std = TW5864_NORMS;
 572        if (v1 & (1 << 7))
 573                i->status |= V4L2_IN_ST_NO_SYNC;
 574        if (!(v1 & (1 << 6)))
 575                i->status |= V4L2_IN_ST_NO_H_LOCK;
 576        if (v1 & (1 << 2))
 577                i->status |= V4L2_IN_ST_NO_SIGNAL;
 578        if (v1 & (1 << 1))
 579                i->status |= V4L2_IN_ST_NO_COLOR;
 580        if (v2 & (1 << 2))
 581                i->status |= V4L2_IN_ST_MACROVISION;
 582
 583        return 0;
 584}
 585
 586static int tw5864_g_input(struct file *file, void *priv, unsigned int *i)
 587{
 588        *i = 0;
 589        return 0;
 590}
 591
 592static int tw5864_s_input(struct file *file, void *priv, unsigned int i)
 593{
 594        if (i)
 595                return -EINVAL;
 596        return 0;
 597}
 598
 599static int tw5864_querycap(struct file *file, void *priv,
 600                           struct v4l2_capability *cap)
 601{
 602        struct tw5864_input *input = video_drvdata(file);
 603
 604        strscpy(cap->driver, "tw5864", sizeof(cap->driver));
 605        snprintf(cap->card, sizeof(cap->card), "TW5864 Encoder %d",
 606                 input->nr);
 607        sprintf(cap->bus_info, "PCI:%s", pci_name(input->root->pci));
 608        return 0;
 609}
 610
 611static int tw5864_querystd(struct file *file, void *priv, v4l2_std_id *std)
 612{
 613        struct tw5864_input *input = video_drvdata(file);
 614        enum tw5864_vid_std tw_std;
 615        int ret;
 616
 617        ret = tw5864_input_std_get(input, &tw_std);
 618        if (ret)
 619                return ret;
 620        *std = tw5864_get_v4l2_std(tw_std);
 621
 622        return 0;
 623}
 624
 625static int tw5864_g_std(struct file *file, void *priv, v4l2_std_id *std)
 626{
 627        struct tw5864_input *input = video_drvdata(file);
 628
 629        *std = input->v4l2_std;
 630        return 0;
 631}
 632
 633static int tw5864_s_std(struct file *file, void *priv, v4l2_std_id std)
 634{
 635        struct tw5864_input *input = video_drvdata(file);
 636        struct tw5864_dev *dev = input->root;
 637
 638        input->v4l2_std = std;
 639        input->std = tw5864_from_v4l2_std(std);
 640        tw_indir_writeb(TW5864_INDIR_VIN_E(input->nr), input->std);
 641        return 0;
 642}
 643
 644static int tw5864_enum_fmt_vid_cap(struct file *file, void *priv,
 645                                   struct v4l2_fmtdesc *f)
 646{
 647        if (f->index)
 648                return -EINVAL;
 649
 650        f->pixelformat = V4L2_PIX_FMT_H264;
 651
 652        return 0;
 653}
 654
 655static int tw5864_subscribe_event(struct v4l2_fh *fh,
 656                                  const struct v4l2_event_subscription *sub)
 657{
 658        switch (sub->type) {
 659        case V4L2_EVENT_MOTION_DET:
 660                /*
 661                 * Allow for up to 30 events (1 second for NTSC) to be stored.
 662                 */
 663                return v4l2_event_subscribe(fh, sub, 30, NULL);
 664        default:
 665                return v4l2_ctrl_subscribe_event(fh, sub);
 666        }
 667}
 668
 669static void tw5864_frame_interval_set(struct tw5864_input *input)
 670{
 671        /*
 672         * This register value seems to follow such approach: In each second
 673         * interval, when processing Nth frame, it checks Nth bit of register
 674         * value and, if the bit is 1, it processes the frame, otherwise the
 675         * frame is discarded.
 676         * So unary representation would work, but more or less equal gaps
 677         * between the frames should be preserved.
 678         *
 679         * For 1 FPS - 0x00000001
 680         * 00000000 00000000 00000000 00000001
 681         *
 682         * For max FPS - set all 25/30 lower bits:
 683         * 00111111 11111111 11111111 11111111 (NTSC)
 684         * 00000001 11111111 11111111 11111111 (PAL)
 685         *
 686         * For half of max FPS - use such pattern:
 687         * 00010101 01010101 01010101 01010101 (NTSC)
 688         * 00000001 01010101 01010101 01010101 (PAL)
 689         *
 690         * Et cetera.
 691         *
 692         * The value supplied to hardware is capped by mask of 25/30 lower bits.
 693         */
 694        struct tw5864_dev *dev = input->root;
 695        u32 unary_framerate = 0;
 696        int shift = 0;
 697        int std_max_fps = input->std == STD_NTSC ? 30 : 25;
 698
 699        for (shift = 0; shift < std_max_fps; shift += input->frame_interval)
 700                unary_framerate |= 0x00000001 << shift;
 701
 702        tw_writel(TW5864_H264EN_RATE_CNTL_LO_WORD(input->nr, 0),
 703                  unary_framerate >> 16);
 704        tw_writel(TW5864_H264EN_RATE_CNTL_HI_WORD(input->nr, 0),
 705                  unary_framerate & 0xffff);
 706}
 707
 708static int tw5864_frameinterval_get(struct tw5864_input *input,
 709                                    struct v4l2_fract *frameinterval)
 710{
 711        struct tw5864_dev *dev = input->root;
 712
 713        switch (input->std) {
 714        case STD_NTSC:
 715                frameinterval->numerator = 1001;
 716                frameinterval->denominator = 30000;
 717                break;
 718        case STD_PAL:
 719        case STD_SECAM:
 720                frameinterval->numerator = 1;
 721                frameinterval->denominator = 25;
 722                break;
 723        default:
 724                dev_warn(&dev->pci->dev, "tw5864_frameinterval_get requested for unknown std %d\n",
 725                         input->std);
 726                return -EINVAL;
 727        }
 728
 729        return 0;
 730}
 731
 732static int tw5864_enum_framesizes(struct file *file, void *priv,
 733                                  struct v4l2_frmsizeenum *fsize)
 734{
 735        struct tw5864_input *input = video_drvdata(file);
 736
 737        if (fsize->index > 0)
 738                return -EINVAL;
 739        if (fsize->pixel_format != V4L2_PIX_FMT_H264)
 740                return -EINVAL;
 741
 742        fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 743        fsize->discrete.width = 704;
 744        fsize->discrete.height = input->std == STD_NTSC ? 480 : 576;
 745
 746        return 0;
 747}
 748
 749static int tw5864_enum_frameintervals(struct file *file, void *priv,
 750                                      struct v4l2_frmivalenum *fintv)
 751{
 752        struct tw5864_input *input = video_drvdata(file);
 753        struct v4l2_fract frameinterval;
 754        int std_max_fps = input->std == STD_NTSC ? 30 : 25;
 755        struct v4l2_frmsizeenum fsize = { .index = fintv->index,
 756                .pixel_format = fintv->pixel_format };
 757        int ret;
 758
 759        ret = tw5864_enum_framesizes(file, priv, &fsize);
 760        if (ret)
 761                return ret;
 762
 763        if (fintv->width != fsize.discrete.width ||
 764            fintv->height != fsize.discrete.height)
 765                return -EINVAL;
 766
 767        fintv->type = V4L2_FRMIVAL_TYPE_STEPWISE;
 768
 769        ret = tw5864_frameinterval_get(input, &frameinterval);
 770        if (ret)
 771                return ret;
 772
 773        fintv->stepwise.step = frameinterval;
 774        fintv->stepwise.min = frameinterval;
 775        fintv->stepwise.max = frameinterval;
 776        fintv->stepwise.max.numerator *= std_max_fps;
 777
 778        return ret;
 779}
 780
 781static int tw5864_g_parm(struct file *file, void *priv,
 782                         struct v4l2_streamparm *sp)
 783{
 784        struct tw5864_input *input = video_drvdata(file);
 785        struct v4l2_captureparm *cp = &sp->parm.capture;
 786        int ret;
 787
 788        cp->capability = V4L2_CAP_TIMEPERFRAME;
 789
 790        ret = tw5864_frameinterval_get(input, &cp->timeperframe);
 791        if (ret)
 792                return ret;
 793
 794        cp->timeperframe.numerator *= input->frame_interval;
 795        cp->capturemode = 0;
 796        cp->readbuffers = 2;
 797
 798        return ret;
 799}
 800
 801static int tw5864_s_parm(struct file *file, void *priv,
 802                         struct v4l2_streamparm *sp)
 803{
 804        struct tw5864_input *input = video_drvdata(file);
 805        struct v4l2_fract *t = &sp->parm.capture.timeperframe;
 806        struct v4l2_fract time_base;
 807        int ret;
 808
 809        ret = tw5864_frameinterval_get(input, &time_base);
 810        if (ret)
 811                return ret;
 812
 813        if (!t->numerator || !t->denominator) {
 814                t->numerator = time_base.numerator * input->frame_interval;
 815                t->denominator = time_base.denominator;
 816        } else if (t->denominator != time_base.denominator) {
 817                t->numerator = t->numerator * time_base.denominator /
 818                        t->denominator;
 819                t->denominator = time_base.denominator;
 820        }
 821
 822        input->frame_interval = t->numerator / time_base.numerator;
 823        if (input->frame_interval < 1)
 824                input->frame_interval = 1;
 825        tw5864_frame_interval_set(input);
 826        return tw5864_g_parm(file, priv, sp);
 827}
 828
 829static const struct v4l2_ctrl_ops tw5864_ctrl_ops = {
 830        .s_ctrl = tw5864_s_ctrl,
 831};
 832
 833static const struct v4l2_file_operations video_fops = {
 834        .owner = THIS_MODULE,
 835        .open = v4l2_fh_open,
 836        .release = vb2_fop_release,
 837        .read = vb2_fop_read,
 838        .poll = vb2_fop_poll,
 839        .mmap = vb2_fop_mmap,
 840        .unlocked_ioctl = video_ioctl2,
 841};
 842
 843#ifdef CONFIG_VIDEO_ADV_DEBUG
 844
 845#define INDIR_SPACE_MAP_SHIFT 0x100000
 846
 847static int tw5864_g_reg(struct file *file, void *fh,
 848                        struct v4l2_dbg_register *reg)
 849{
 850        struct tw5864_input *input = video_drvdata(file);
 851        struct tw5864_dev *dev = input->root;
 852
 853        if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
 854                if (reg->reg > 0x87fff)
 855                        return -EINVAL;
 856                reg->size = 4;
 857                reg->val = tw_readl(reg->reg);
 858        } else {
 859                __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
 860
 861                if (indir_addr > 0xefe)
 862                        return -EINVAL;
 863                reg->size = 1;
 864                reg->val = tw_indir_readb(reg->reg);
 865        }
 866        return 0;
 867}
 868
 869static int tw5864_s_reg(struct file *file, void *fh,
 870                        const struct v4l2_dbg_register *reg)
 871{
 872        struct tw5864_input *input = video_drvdata(file);
 873        struct tw5864_dev *dev = input->root;
 874
 875        if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
 876                if (reg->reg > 0x87fff)
 877                        return -EINVAL;
 878                tw_writel(reg->reg, reg->val);
 879        } else {
 880                __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
 881
 882                if (indir_addr > 0xefe)
 883                        return -EINVAL;
 884                tw_indir_writeb(reg->reg, reg->val);
 885        }
 886        return 0;
 887}
 888#endif
 889
 890static const struct v4l2_ioctl_ops video_ioctl_ops = {
 891        .vidioc_querycap = tw5864_querycap,
 892        .vidioc_enum_fmt_vid_cap = tw5864_enum_fmt_vid_cap,
 893        .vidioc_reqbufs = vb2_ioctl_reqbufs,
 894        .vidioc_create_bufs = vb2_ioctl_create_bufs,
 895        .vidioc_querybuf = vb2_ioctl_querybuf,
 896        .vidioc_qbuf = vb2_ioctl_qbuf,
 897        .vidioc_dqbuf = vb2_ioctl_dqbuf,
 898        .vidioc_expbuf = vb2_ioctl_expbuf,
 899        .vidioc_querystd = tw5864_querystd,
 900        .vidioc_s_std = tw5864_s_std,
 901        .vidioc_g_std = tw5864_g_std,
 902        .vidioc_enum_input = tw5864_enum_input,
 903        .vidioc_g_input = tw5864_g_input,
 904        .vidioc_s_input = tw5864_s_input,
 905        .vidioc_streamon = vb2_ioctl_streamon,
 906        .vidioc_streamoff = vb2_ioctl_streamoff,
 907        .vidioc_try_fmt_vid_cap = tw5864_fmt_vid_cap,
 908        .vidioc_s_fmt_vid_cap = tw5864_fmt_vid_cap,
 909        .vidioc_g_fmt_vid_cap = tw5864_fmt_vid_cap,
 910        .vidioc_log_status = v4l2_ctrl_log_status,
 911        .vidioc_subscribe_event = tw5864_subscribe_event,
 912        .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 913        .vidioc_enum_framesizes = tw5864_enum_framesizes,
 914        .vidioc_enum_frameintervals = tw5864_enum_frameintervals,
 915        .vidioc_s_parm = tw5864_s_parm,
 916        .vidioc_g_parm = tw5864_g_parm,
 917#ifdef CONFIG_VIDEO_ADV_DEBUG
 918        .vidioc_g_register = tw5864_g_reg,
 919        .vidioc_s_register = tw5864_s_reg,
 920#endif
 921};
 922
 923static const struct video_device tw5864_video_template = {
 924        .name = "tw5864_video",
 925        .fops = &video_fops,
 926        .ioctl_ops = &video_ioctl_ops,
 927        .release = video_device_release_empty,
 928        .tvnorms = TW5864_NORMS,
 929        .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
 930                V4L2_CAP_STREAMING,
 931};
 932
 933/* Motion Detection Threshold matrix */
 934static const struct v4l2_ctrl_config tw5864_md_thresholds = {
 935        .ops = &tw5864_ctrl_ops,
 936        .id = V4L2_CID_DETECT_MD_THRESHOLD_GRID,
 937        .dims = {MD_CELLS_HOR, MD_CELLS_VERT},
 938        .def = 14,
 939        /* See tw5864_md_metric_from_mvd() */
 940        .max = 2 * 0x0f,
 941        .step = 1,
 942};
 943
 944static int tw5864_video_input_init(struct tw5864_input *dev, int video_nr);
 945static void tw5864_video_input_fini(struct tw5864_input *dev);
 946static void tw5864_encoder_tables_upload(struct tw5864_dev *dev);
 947
 948int tw5864_video_init(struct tw5864_dev *dev, int *video_nr)
 949{
 950        int i;
 951        int ret;
 952        unsigned long flags;
 953        int last_dma_allocated = -1;
 954        int last_input_nr_registered = -1;
 955
 956        for (i = 0; i < H264_BUF_CNT; i++) {
 957                struct tw5864_h264_frame *frame = &dev->h264_buf[i];
 958
 959                frame->vlc.addr = dma_alloc_coherent(&dev->pci->dev,
 960                                                     H264_VLC_BUF_SIZE,
 961                                                     &frame->vlc.dma_addr,
 962                                                     GFP_KERNEL | GFP_DMA32);
 963                if (!frame->vlc.addr) {
 964                        dev_err(&dev->pci->dev, "dma alloc fail\n");
 965                        ret = -ENOMEM;
 966                        goto free_dma;
 967                }
 968                frame->mv.addr = dma_alloc_coherent(&dev->pci->dev,
 969                                                    H264_MV_BUF_SIZE,
 970                                                    &frame->mv.dma_addr,
 971                                                    GFP_KERNEL | GFP_DMA32);
 972                if (!frame->mv.addr) {
 973                        dev_err(&dev->pci->dev, "dma alloc fail\n");
 974                        ret = -ENOMEM;
 975                        dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
 976                                          frame->vlc.addr, frame->vlc.dma_addr);
 977                        goto free_dma;
 978                }
 979                last_dma_allocated = i;
 980        }
 981
 982        tw5864_encoder_tables_upload(dev);
 983
 984        /* Picture is distorted without this block */
 985        /* use falling edge to sample 54M to 108M */
 986        tw_indir_writeb(TW5864_INDIR_VD_108_POL, TW5864_INDIR_VD_108_POL_BOTH);
 987        tw_indir_writeb(TW5864_INDIR_CLK0_SEL, 0x00);
 988
 989        tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL0, 0x02);
 990        tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL1, 0x02);
 991        tw_indir_writeb(TW5864_INDIR_DDRA_DLL_CLK90_SEL, 0x02);
 992        tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL0, 0x02);
 993        tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL1, 0x02);
 994        tw_indir_writeb(TW5864_INDIR_DDRB_DLL_CLK90_SEL, 0x02);
 995
 996        /* video input reset */
 997        tw_indir_writeb(TW5864_INDIR_RESET, 0);
 998        tw_indir_writeb(TW5864_INDIR_RESET, TW5864_INDIR_RESET_VD |
 999                        TW5864_INDIR_RESET_DLL | TW5864_INDIR_RESET_MUX_CORE);
1000        msleep(20);
1001
1002        /*
1003         * Select Part A mode for all channels.
1004         * tw_setl instead of tw_clearl for Part B mode.
1005         *
1006         * I guess "Part B" is primarily for downscaled version of same channel
1007         * which goes in Part A of same bus
1008         */
1009        tw_writel(TW5864_FULL_HALF_MODE_SEL, 0);
1010
1011        tw_indir_writeb(TW5864_INDIR_PV_VD_CK_POL,
1012                        TW5864_INDIR_PV_VD_CK_POL_VD(0) |
1013                        TW5864_INDIR_PV_VD_CK_POL_VD(1) |
1014                        TW5864_INDIR_PV_VD_CK_POL_VD(2) |
1015                        TW5864_INDIR_PV_VD_CK_POL_VD(3));
1016
1017        spin_lock_irqsave(&dev->slock, flags);
1018        dev->encoder_busy = 0;
1019        dev->h264_buf_r_index = 0;
1020        dev->h264_buf_w_index = 0;
1021        tw_writel(TW5864_VLC_STREAM_BASE_ADDR,
1022                  dev->h264_buf[dev->h264_buf_w_index].vlc.dma_addr);
1023        tw_writel(TW5864_MV_STREAM_BASE_ADDR,
1024                  dev->h264_buf[dev->h264_buf_w_index].mv.dma_addr);
1025        spin_unlock_irqrestore(&dev->slock, flags);
1026
1027        tw_writel(TW5864_SEN_EN_CH, 0x000f);
1028        tw_writel(TW5864_H264EN_CH_EN, 0x000f);
1029
1030        tw_writel(TW5864_H264EN_BUS0_MAP, 0x00000000);
1031        tw_writel(TW5864_H264EN_BUS1_MAP, 0x00001111);
1032        tw_writel(TW5864_H264EN_BUS2_MAP, 0x00002222);
1033        tw_writel(TW5864_H264EN_BUS3_MAP, 0x00003333);
1034
1035        /*
1036         * Quote from Intersil (manufacturer):
1037         * 0x0038 is managed by HW, and by default it won't pass the pointer set
1038         * at 0x0010. So if you don't do encoding, 0x0038 should stay at '3'
1039         * (with 4 frames in buffer). If you encode one frame and then move
1040         * 0x0010 to '1' for example, HW will take one more frame and set it to
1041         * buffer #0, and then you should see 0x0038 is set to '0'.  There is
1042         * only one HW encoder engine, so 4 channels cannot get encoded
1043         * simultaneously. But each channel does have its own buffer (for
1044         * original frames and reconstructed frames). So there is no problem to
1045         * manage encoding for 4 channels at same time and no need to force
1046         * I-frames in switching channels.
1047         * End of quote.
1048         *
1049         * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0 (for any channel), we
1050         * have no "rolling" (until we change this value).
1051         * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0x3, it starts to roll
1052         * continuously together with 0x0038.
1053         */
1054        tw_writel(TW5864_ENC_BUF_PTR_REC1, 0x00ff);
1055        tw_writel(TW5864_PCI_INTTM_SCALE, 0);
1056
1057        tw_writel(TW5864_INTERLACING, TW5864_DI_EN);
1058        tw_writel(TW5864_MASTER_ENB_REG, TW5864_PCI_VLC_INTR_ENB);
1059        tw_writel(TW5864_PCI_INTR_CTL,
1060                  TW5864_TIMER_INTR_ENB | TW5864_PCI_MAST_ENB |
1061                  TW5864_MVD_VLC_MAST_ENB);
1062
1063        dev->irqmask |= TW5864_INTR_VLC_DONE | TW5864_INTR_TIMER;
1064        tw5864_irqmask_apply(dev);
1065
1066        tasklet_setup(&dev->tasklet, tw5864_handle_frame_task);
1067
1068        for (i = 0; i < TW5864_INPUTS; i++) {
1069                dev->inputs[i].root = dev;
1070                dev->inputs[i].nr = i;
1071                ret = tw5864_video_input_init(&dev->inputs[i], video_nr[i]);
1072                if (ret)
1073                        goto fini_video_inputs;
1074                last_input_nr_registered = i;
1075        }
1076
1077        return 0;
1078
1079fini_video_inputs:
1080        for (i = last_input_nr_registered; i >= 0; i--)
1081                tw5864_video_input_fini(&dev->inputs[i]);
1082
1083        tasklet_kill(&dev->tasklet);
1084
1085free_dma:
1086        for (i = last_dma_allocated; i >= 0; i--) {
1087                dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1088                                  dev->h264_buf[i].vlc.addr,
1089                                  dev->h264_buf[i].vlc.dma_addr);
1090                dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1091                                  dev->h264_buf[i].mv.addr,
1092                                  dev->h264_buf[i].mv.dma_addr);
1093        }
1094
1095        return ret;
1096}
1097
1098static int tw5864_video_input_init(struct tw5864_input *input, int video_nr)
1099{
1100        struct tw5864_dev *dev = input->root;
1101        int ret;
1102        struct v4l2_ctrl_handler *hdl = &input->hdl;
1103
1104        mutex_init(&input->lock);
1105        spin_lock_init(&input->slock);
1106
1107        /* setup video buffers queue */
1108        INIT_LIST_HEAD(&input->active);
1109        input->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1110        input->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1111        input->vidq.io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
1112        input->vidq.ops = &tw5864_video_qops;
1113        input->vidq.mem_ops = &vb2_dma_contig_memops;
1114        input->vidq.drv_priv = input;
1115        input->vidq.gfp_flags = 0;
1116        input->vidq.buf_struct_size = sizeof(struct tw5864_buf);
1117        input->vidq.lock = &input->lock;
1118        input->vidq.min_buffers_needed = 2;
1119        input->vidq.dev = &input->root->pci->dev;
1120        ret = vb2_queue_init(&input->vidq);
1121        if (ret)
1122                goto free_mutex;
1123
1124        input->vdev = tw5864_video_template;
1125        input->vdev.v4l2_dev = &input->root->v4l2_dev;
1126        input->vdev.lock = &input->lock;
1127        input->vdev.queue = &input->vidq;
1128        video_set_drvdata(&input->vdev, input);
1129
1130        /* Initialize the device control structures */
1131        v4l2_ctrl_handler_init(hdl, 6);
1132        v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1133                          V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1134        v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1135                          V4L2_CID_CONTRAST, 0, 255, 1, 100);
1136        v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1137                          V4L2_CID_SATURATION, 0, 255, 1, 128);
1138        v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_HUE, -128, 127, 1, 0);
1139        v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1140                          1, MAX_GOP_SIZE, 1, GOP_SIZE);
1141        v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1142                          V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 28, 51, 1, QP_VALUE);
1143        v4l2_ctrl_new_std_menu(hdl, &tw5864_ctrl_ops,
1144                               V4L2_CID_DETECT_MD_MODE,
1145                               V4L2_DETECT_MD_MODE_THRESHOLD_GRID, 0,
1146                               V4L2_DETECT_MD_MODE_DISABLED);
1147        v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1148                          V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD,
1149                          tw5864_md_thresholds.min, tw5864_md_thresholds.max,
1150                          tw5864_md_thresholds.step, tw5864_md_thresholds.def);
1151        input->md_threshold_grid_ctrl =
1152                v4l2_ctrl_new_custom(hdl, &tw5864_md_thresholds, NULL);
1153        if (hdl->error) {
1154                ret = hdl->error;
1155                goto free_v4l2_hdl;
1156        }
1157        input->vdev.ctrl_handler = hdl;
1158        v4l2_ctrl_handler_setup(hdl);
1159
1160        input->qp = QP_VALUE;
1161        input->gop = GOP_SIZE;
1162        input->frame_interval = 1;
1163
1164        ret = video_register_device(&input->vdev, VFL_TYPE_VIDEO, video_nr);
1165        if (ret)
1166                goto free_v4l2_hdl;
1167
1168        dev_info(&input->root->pci->dev, "Registered video device %s\n",
1169                 video_device_node_name(&input->vdev));
1170
1171        /*
1172         * Set default video standard. Doesn't matter which, the detected value
1173         * will be found out by VIDIOC_QUERYSTD handler.
1174         */
1175        input->v4l2_std = V4L2_STD_NTSC_M;
1176        input->std = STD_NTSC;
1177
1178        tw_indir_writeb(TW5864_INDIR_VIN_E(video_nr), 0x07);
1179        /* to initiate auto format recognition */
1180        tw_indir_writeb(TW5864_INDIR_VIN_F(video_nr), 0xff);
1181
1182        return 0;
1183
1184free_v4l2_hdl:
1185        v4l2_ctrl_handler_free(hdl);
1186free_mutex:
1187        mutex_destroy(&input->lock);
1188
1189        return ret;
1190}
1191
1192static void tw5864_video_input_fini(struct tw5864_input *dev)
1193{
1194        vb2_video_unregister_device(&dev->vdev);
1195        v4l2_ctrl_handler_free(&dev->hdl);
1196}
1197
1198void tw5864_video_fini(struct tw5864_dev *dev)
1199{
1200        int i;
1201
1202        tasklet_kill(&dev->tasklet);
1203
1204        for (i = 0; i < TW5864_INPUTS; i++)
1205                tw5864_video_input_fini(&dev->inputs[i]);
1206
1207        for (i = 0; i < H264_BUF_CNT; i++) {
1208                dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1209                                  dev->h264_buf[i].vlc.addr,
1210                                  dev->h264_buf[i].vlc.dma_addr);
1211                dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1212                                  dev->h264_buf[i].mv.addr,
1213                                  dev->h264_buf[i].mv.dma_addr);
1214        }
1215}
1216
1217void tw5864_prepare_frame_headers(struct tw5864_input *input)
1218{
1219        struct tw5864_buf *vb = input->vb;
1220        u8 *dst;
1221        size_t dst_space;
1222        unsigned long flags;
1223
1224        if (!vb) {
1225                spin_lock_irqsave(&input->slock, flags);
1226                if (list_empty(&input->active)) {
1227                        spin_unlock_irqrestore(&input->slock, flags);
1228                        input->vb = NULL;
1229                        return;
1230                }
1231                vb = list_first_entry(&input->active, struct tw5864_buf, list);
1232                list_del(&vb->list);
1233                spin_unlock_irqrestore(&input->slock, flags);
1234        }
1235
1236        dst = vb2_plane_vaddr(&vb->vb.vb2_buf, 0);
1237        dst_space = vb2_plane_size(&vb->vb.vb2_buf, 0);
1238
1239        /*
1240         * Low-level bitstream writing functions don't have a fine way to say
1241         * correctly that supplied buffer is too small. So we just check there
1242         * and warn, and don't care at lower level.
1243         * Currently all headers take below 32 bytes.
1244         * The buffer is supposed to have plenty of free space at this point,
1245         * anyway.
1246         */
1247        if (WARN_ON_ONCE(dst_space < 128))
1248                return;
1249
1250        /*
1251         * Generate H264 headers:
1252         * If this is first frame, put SPS and PPS
1253         */
1254        if (input->frame_gop_seqno == 0)
1255                tw5864_h264_put_stream_header(&dst, &dst_space, input->qp,
1256                                              input->width, input->height);
1257
1258        /* Put slice header */
1259        tw5864_h264_put_slice_header(&dst, &dst_space, input->h264_idr_pic_id,
1260                                     input->frame_gop_seqno,
1261                                     &input->tail_nb_bits, &input->tail);
1262        input->vb = vb;
1263        input->buf_cur_ptr = dst;
1264        input->buf_cur_space_left = dst_space;
1265}
1266
1267/*
1268 * Returns heuristic motion detection metric value from known components of
1269 * hardware-provided Motion Vector Data.
1270 */
1271static unsigned int tw5864_md_metric_from_mvd(u32 mvd)
1272{
1273        /*
1274         * Format of motion vector data exposed by tw5864, according to
1275         * manufacturer:
1276         * mv_x 10 bits
1277         * mv_y 10 bits
1278         * non_zero_members 8 bits
1279         * mb_type 3 bits
1280         * reserved 1 bit
1281         *
1282         * non_zero_members: number of non-zero residuals in each macro block
1283         * after quantization
1284         *
1285         * unsigned int reserved = mvd >> 31;
1286         * unsigned int mb_type = (mvd >> 28) & 0x7;
1287         * unsigned int non_zero_members = (mvd >> 20) & 0xff;
1288         */
1289        unsigned int mv_y = (mvd >> 10) & 0x3ff;
1290        unsigned int mv_x = mvd & 0x3ff;
1291
1292        /* heuristic: */
1293        mv_x &= 0x0f;
1294        mv_y &= 0x0f;
1295
1296        return mv_y + mv_x;
1297}
1298
1299static int tw5864_is_motion_triggered(struct tw5864_h264_frame *frame)
1300{
1301        struct tw5864_input *input = frame->input;
1302        u32 *mv = (u32 *)frame->mv.addr;
1303        int i;
1304        int detected = 0;
1305
1306        for (i = 0; i < MD_CELLS; i++) {
1307                const u16 thresh = input->md_threshold_grid_values[i];
1308                const unsigned int metric = tw5864_md_metric_from_mvd(mv[i]);
1309
1310                if (metric > thresh)
1311                        detected = 1;
1312
1313                if (detected)
1314                        break;
1315        }
1316        return detected;
1317}
1318
1319static void tw5864_handle_frame_task(struct tasklet_struct *t)
1320{
1321        struct tw5864_dev *dev = from_tasklet(dev, t, tasklet);
1322        unsigned long flags;
1323        int batch_size = H264_BUF_CNT;
1324
1325        spin_lock_irqsave(&dev->slock, flags);
1326        while (dev->h264_buf_r_index != dev->h264_buf_w_index && batch_size--) {
1327                struct tw5864_h264_frame *frame =
1328                        &dev->h264_buf[dev->h264_buf_r_index];
1329
1330                spin_unlock_irqrestore(&dev->slock, flags);
1331                dma_sync_single_for_cpu(&dev->pci->dev, frame->vlc.dma_addr,
1332                                        H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1333                dma_sync_single_for_cpu(&dev->pci->dev, frame->mv.dma_addr,
1334                                        H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1335                tw5864_handle_frame(frame);
1336                dma_sync_single_for_device(&dev->pci->dev, frame->vlc.dma_addr,
1337                                           H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1338                dma_sync_single_for_device(&dev->pci->dev, frame->mv.dma_addr,
1339                                           H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1340                spin_lock_irqsave(&dev->slock, flags);
1341
1342                dev->h264_buf_r_index++;
1343                dev->h264_buf_r_index %= H264_BUF_CNT;
1344        }
1345        spin_unlock_irqrestore(&dev->slock, flags);
1346}
1347
1348#ifdef DEBUG
1349static u32 tw5864_vlc_checksum(u32 *data, int len)
1350{
1351        u32 val, count_len = len;
1352
1353        val = *data++;
1354        while (((count_len >> 2) - 1) > 0) {
1355                val ^= *data++;
1356                count_len -= 4;
1357        }
1358        val ^= htonl((len >> 2));
1359        return val;
1360}
1361#endif
1362
1363static void tw5864_handle_frame(struct tw5864_h264_frame *frame)
1364{
1365#define SKIP_VLCBUF_BYTES 3
1366        struct tw5864_input *input = frame->input;
1367        struct tw5864_dev *dev = input->root;
1368        struct tw5864_buf *vb;
1369        struct vb2_v4l2_buffer *v4l2_buf;
1370        int frame_len = frame->vlc_len - SKIP_VLCBUF_BYTES;
1371        u8 *dst = input->buf_cur_ptr;
1372        u8 tail_mask, vlc_mask = 0;
1373        int i;
1374        u8 vlc_first_byte = ((u8 *)(frame->vlc.addr + SKIP_VLCBUF_BYTES))[0];
1375        unsigned long flags;
1376        int zero_run;
1377        u8 *src;
1378        u8 *src_end;
1379
1380#ifdef DEBUG
1381        if (frame->checksum !=
1382            tw5864_vlc_checksum((u32 *)frame->vlc.addr, frame_len))
1383                dev_err(&dev->pci->dev,
1384                        "Checksum of encoded frame doesn't match!\n");
1385#endif
1386
1387        spin_lock_irqsave(&input->slock, flags);
1388        vb = input->vb;
1389        input->vb = NULL;
1390        spin_unlock_irqrestore(&input->slock, flags);
1391
1392        if (!vb) { /* Gone because of disabling */
1393                dev_dbg(&dev->pci->dev, "vb is empty, dropping frame\n");
1394                return;
1395        }
1396
1397        v4l2_buf = to_vb2_v4l2_buffer(&vb->vb.vb2_buf);
1398
1399        /*
1400         * Check for space.
1401         * Mind the overhead of startcode emulation prevention.
1402         */
1403        if (input->buf_cur_space_left < frame_len * 5 / 4) {
1404                dev_err_once(&dev->pci->dev,
1405                             "Left space in vb2 buffer, %d bytes, is less than considered safely enough to put frame of length %d. Dropping this frame.\n",
1406                             input->buf_cur_space_left, frame_len);
1407                return;
1408        }
1409
1410        for (i = 0; i < 8 - input->tail_nb_bits; i++)
1411                vlc_mask |= 1 << i;
1412        tail_mask = (~vlc_mask) & 0xff;
1413
1414        dst[0] = (input->tail & tail_mask) | (vlc_first_byte & vlc_mask);
1415        frame_len--;
1416        dst++;
1417
1418        /* H.264 startcode emulation prevention */
1419        src = frame->vlc.addr + SKIP_VLCBUF_BYTES + 1;
1420        src_end = src + frame_len;
1421        zero_run = 0;
1422        for (; src < src_end; src++) {
1423                if (zero_run < 2) {
1424                        if (*src == 0)
1425                                ++zero_run;
1426                        else
1427                                zero_run = 0;
1428                } else {
1429                        if ((*src & ~0x03) == 0)
1430                                *dst++ = 0x03;
1431                        zero_run = *src == 0;
1432                }
1433                *dst++ = *src;
1434        }
1435
1436        vb2_set_plane_payload(&vb->vb.vb2_buf, 0,
1437                              dst - (u8 *)vb2_plane_vaddr(&vb->vb.vb2_buf, 0));
1438
1439        vb->vb.vb2_buf.timestamp = frame->timestamp;
1440        v4l2_buf->field = V4L2_FIELD_INTERLACED;
1441        v4l2_buf->sequence = frame->seqno;
1442
1443        /* Check for motion flags */
1444        if (frame->gop_seqno /* P-frame */ &&
1445            tw5864_is_motion_triggered(frame)) {
1446                struct v4l2_event ev = {
1447                        .type = V4L2_EVENT_MOTION_DET,
1448                        .u.motion_det = {
1449                                .flags = V4L2_EVENT_MD_FL_HAVE_FRAME_SEQ,
1450                                .frame_sequence = v4l2_buf->sequence,
1451                        },
1452                };
1453
1454                v4l2_event_queue(&input->vdev, &ev);
1455        }
1456
1457        vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
1458}
1459
1460static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std std)
1461{
1462        switch (std) {
1463        case STD_NTSC:    return V4L2_STD_NTSC_M;
1464        case STD_PAL:     return V4L2_STD_PAL_B;
1465        case STD_SECAM:   return V4L2_STD_SECAM_B;
1466        case STD_NTSC443: return V4L2_STD_NTSC_443;
1467        case STD_PAL_M:   return V4L2_STD_PAL_M;
1468        case STD_PAL_CN:  return V4L2_STD_PAL_Nc;
1469        case STD_PAL_60:  return V4L2_STD_PAL_60;
1470        case STD_INVALID: return V4L2_STD_UNKNOWN;
1471        }
1472        return 0;
1473}
1474
1475static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std)
1476{
1477        if (v4l2_std & V4L2_STD_NTSC_M)
1478                return STD_NTSC;
1479        if (v4l2_std & V4L2_STD_PAL_B)
1480                return STD_PAL;
1481        if (v4l2_std & V4L2_STD_SECAM_B)
1482                return STD_SECAM;
1483        if (v4l2_std & V4L2_STD_NTSC_443)
1484                return STD_NTSC443;
1485        if (v4l2_std & V4L2_STD_PAL_M)
1486                return STD_PAL_M;
1487        if (v4l2_std & V4L2_STD_PAL_Nc)
1488                return STD_PAL_CN;
1489        if (v4l2_std & V4L2_STD_PAL_60)
1490                return STD_PAL_60;
1491
1492        return STD_INVALID;
1493}
1494
1495static void tw5864_encoder_tables_upload(struct tw5864_dev *dev)
1496{
1497        int i;
1498
1499        tw_writel(TW5864_VLC_RD, 0x1);
1500        for (i = 0; i < VLC_LOOKUP_TABLE_LEN; i++) {
1501                tw_writel((TW5864_VLC_STREAM_MEM_START + i * 4),
1502                          encoder_vlc_lookup_table[i]);
1503        }
1504        tw_writel(TW5864_VLC_RD, 0x0);
1505
1506        for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1507                tw_writel((TW5864_QUAN_TAB + i * 4),
1508                          forward_quantization_table[i]);
1509        }
1510
1511        for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1512                tw_writel((TW5864_QUAN_TAB + i * 4),
1513                          inverse_quantization_table[i]);
1514        }
1515}
1516
lxr.linux.no kindly hosted by Redpill Linpro AS, provider of Linux consulting and operations services since 1995.