drivers/media/pci/tw5864/tw5864-video.c

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