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         /*
 334          * Crop width from 720 to 704.
 335          * Above register settings need value 720 involved.
 336          */
 337         input->width = 704;
 338         tw_indir_writeb(TW5864_INDIR_CROP_ETC,
 339                         tw_indir_readb(TW5864_INDIR_CROP_ETC) |
 340                         TW5864_INDIR_CROP_ETC_CROP_EN);
 341
 342         tw_writel(TW5864_DSP_PIC_MAX_MB,
 343                   ((input->width / 16) << 8) | (input->height / 16));
 344
 345         tw_writel(TW5864_FRAME_WIDTH_BUS_A(nr),
 346                   frame_width_bus_value);
 347         tw_writel(TW5864_FRAME_WIDTH_BUS_B(nr),
 348                   frame_width_bus_value);
 349         tw_writel(TW5864_FRAME_HEIGHT_BUS_A(nr),
 350                   frame_height_bus_value);
 351         tw_writel(TW5864_FRAME_HEIGHT_BUS_B(nr),
 352                   (frame_height_bus_value + 1) / 2 - 1);
 353
 354         tw5864_frame_interval_set(input);
 355
 356         if (downscale_enabled)
 357                 tw_setl(TW5864_H264EN_CH_DNS, 1 << nr);
 358
 359         tw_mask_shift_writel(TW5864_H264EN_CH_FMT_REG1, 0x3, 2 * nr,
 360                              fmt_reg_value);
 361
 362         tw_mask_shift_writel((nr < 2
 363                               ? TW5864_H264EN_RATE_MAX_LINE_REG1
 364                               : TW5864_H264EN_RATE_MAX_LINE_REG2),
 365                              0x1f, 5 * (nr % 2),
 366                              input->std == STD_NTSC ? 29 : 24);
 367
 368         tw_mask_shift_writel((nr < 2) ? TW5864_FRAME_BUS1 :
 369                              TW5864_FRAME_BUS2, 0xff, (nr % 2) * 8,
 370                              reg_frame_bus);
 371
 372         spin_lock_irqsave(&dev->slock, flags);
 373         input->enabled = 1;
 374         spin_unlock_irqrestore(&dev->slock, flags);
 375
 376         return 0;
 377 }
 378
 379 void tw5864_request_encoded_frame(struct tw5864_input *input)
 380 {
 381         struct tw5864_dev *dev = input->root;
 382         u32 enc_buf_id_new;
 383
 384         tw_setl(TW5864_DSP_CODEC, TW5864_CIF_MAP_MD | TW5864_HD1_MAP_MD);
 385         tw_writel(TW5864_EMU, input->reg_emu);
 386         tw_writel(TW5864_INTERLACING, input->reg_interlacing);
 387         tw_writel(TW5864_DSP, input->reg_dsp);
 388
 389         tw_writel(TW5864_DSP_QP, input->reg_dsp_qp);
 390         tw_writel(TW5864_DSP_REF_MVP_LAMBDA, input->reg_dsp_ref_mvp_lambda);
 391         tw_writel(TW5864_DSP_I4x4_WEIGHT, input->reg_dsp_i4x4_weight);
 392         tw_mask_shift_writel(TW5864_DSP_INTRA_MODE, TW5864_DSP_INTRA_MODE_MASK,
 393                              TW5864_DSP_INTRA_MODE_SHIFT,
 394                              TW5864_DSP_INTRA_MODE_16x16);
 395
 396         if (input->frame_gop_seqno == 0) {
 397                 /* Produce I-frame */
 398                 tw_writel(TW5864_MOTION_SEARCH_ETC, TW5864_INTRA_EN);
 399                 input->h264_idr_pic_id++;
 400                 input->h264_idr_pic_id &= TW5864_DSP_REF_FRM;
 401         } else {
 402                 /* Produce P-frame */
 403                 tw_writel(TW5864_MOTION_SEARCH_ETC, TW5864_INTRA_EN |
 404                           TW5864_ME_EN | BIT(5) /* SRCH_OPT default */);
 405         }
 406         tw5864_prepare_frame_headers(input);
 407         tw_writel(TW5864_VLC,
 408                   TW5864_VLC_PCI_SEL |
 409                   ((input->tail_nb_bits + 24) << TW5864_VLC_BIT_ALIGN_SHIFT) |
 410                   input->reg_dsp_qp);
 411
 412         enc_buf_id_new = tw_mask_shift_readl(TW5864_ENC_BUF_PTR_REC1, 0x3,
 413                                              2 * input->nr);
 414         tw_writel(TW5864_DSP_ENC_ORG_PTR_REG,
 415                   enc_buf_id_new << TW5864_DSP_ENC_ORG_PTR_SHIFT);
 416         tw_writel(TW5864_DSP_ENC_REC,
 417                   enc_buf_id_new << 12 | ((enc_buf_id_new + 3) & 3));
 418
 419         tw_writel(TW5864_SLICE, TW5864_START_NSLICE);
 420         tw_writel(TW5864_SLICE, 0);
 421 }
 422
 423 static int tw5864_disable_input(struct tw5864_input *input)
 424 {
 425         struct tw5864_dev *dev = input->root;
 426         unsigned long flags;
 427
 428         dev_dbg(&dev->pci->dev, "Disabling channel %d\n", input->nr);
 429
 430         spin_lock_irqsave(&dev->slock, flags);
 431         input->enabled = 0;
 432         spin_unlock_irqrestore(&dev->slock, flags);
 433         return 0;
 434 }
 435
 436 static int tw5864_start_streaming(struct vb2_queue *q, unsigned int count)
 437 {
 438         struct tw5864_input *input = vb2_get_drv_priv(q);
 439         int ret;
 440
 441         ret = tw5864_enable_input(input);
 442         if (!ret)
 443                 return 0;
 444
 445         while (!list_empty(&input->active)) {
 446                 struct tw5864_buf *buf = list_entry(input->active.next,
 447                                                     struct tw5864_buf, list);
 448
 449                 list_del(&buf->list);
 450                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
 451         }
 452         return ret;
 453 }
 454
 455 static void tw5864_stop_streaming(struct vb2_queue *q)
 456 {
 457         unsigned long flags;
 458         struct tw5864_input *input = vb2_get_drv_priv(q);
 459
 460         tw5864_disable_input(input);
 461
 462         spin_lock_irqsave(&input->slock, flags);
 463         if (input->vb) {
 464                 vb2_buffer_done(&input->vb->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 465                 input->vb = NULL;
 466         }
 467         while (!list_empty(&input->active)) {
 468                 struct tw5864_buf *buf = list_entry(input->active.next,
 469                                                     struct tw5864_buf, list);
 470
 471                 list_del(&buf->list);
 472                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 473         }
 474         spin_unlock_irqrestore(&input->slock, flags);
 475 }
 476
 477 static const struct vb2_ops tw5864_video_qops = {
 478         .queue_setup = tw5864_queue_setup,
 479         .buf_queue = tw5864_buf_queue,
 480         .start_streaming = tw5864_start_streaming,
 481         .stop_streaming = tw5864_stop_streaming,
 482         .wait_prepare = vb2_ops_wait_prepare,
 483         .wait_finish = vb2_ops_wait_finish,
 484 };
 485
 486 static int tw5864_s_ctrl(struct v4l2_ctrl *ctrl)
 487 {
 488         struct tw5864_input *input =
 489                 container_of(ctrl->handler, struct tw5864_input, hdl);
 490         struct tw5864_dev *dev = input->root;
 491         unsigned long flags;
 492
 493         switch (ctrl->id) {
 494         case V4L2_CID_BRIGHTNESS:
 495                 tw_indir_writeb(TW5864_INDIR_VIN_A_BRIGHT(input->nr),
 496                                 (u8)ctrl->val);
 497                 break;
 498         case V4L2_CID_HUE:
 499                 tw_indir_writeb(TW5864_INDIR_VIN_7_HUE(input->nr),
 500                                 (u8)ctrl->val);
 501                 break;
 502         case V4L2_CID_CONTRAST:
 503                 tw_indir_writeb(TW5864_INDIR_VIN_9_CNTRST(input->nr),
 504                                 (u8)ctrl->val);
 505                 break;
 506         case V4L2_CID_SATURATION:
 507                 tw_indir_writeb(TW5864_INDIR_VIN_B_SAT_U(input->nr),
 508                                 (u8)ctrl->val);
 509                 tw_indir_writeb(TW5864_INDIR_VIN_C_SAT_V(input->nr),
 510                                 (u8)ctrl->val);
 511                 break;
 512         case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
 513                 input->gop = ctrl->val;
 514                 return 0;
 515         case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
 516                 spin_lock_irqsave(&input->slock, flags);
 517                 input->qp = ctrl->val;
 518                 input->reg_dsp_qp = input->qp;
 519                 input->reg_dsp_ref_mvp_lambda = lambda_lookup_table[input->qp];
 520                 input->reg_dsp_i4x4_weight = intra4x4_lambda3[input->qp];
 521                 spin_unlock_irqrestore(&input->slock, flags);
 522                 return 0;
 523         case V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD:
 524                 memset(input->md_threshold_grid_values, ctrl->val,
 525                        sizeof(input->md_threshold_grid_values));
 526                 return 0;
 527         case V4L2_CID_DETECT_MD_MODE:
 528                 return 0;
 529         case V4L2_CID_DETECT_MD_THRESHOLD_GRID:
 530                 /* input->md_threshold_grid_ctrl->p_new.p_u16 contains data */
 531                 memcpy(input->md_threshold_grid_values,
 532                        input->md_threshold_grid_ctrl->p_new.p_u16,
 533                        sizeof(input->md_threshold_grid_values));
 534                 return 0;
 535         }
 536         return 0;
 537 }
 538
 539 static int tw5864_fmt_vid_cap(struct file *file, void *priv,
 540                               struct v4l2_format *f)
 541 {
 542         struct tw5864_input *input = video_drvdata(file);
 543
 544         f->fmt.pix.width = 704;
 545         switch (input->std) {
 546         default:
 547                 WARN_ON_ONCE(1);
 548                 return -EINVAL;
 549         case STD_NTSC:
 550                 f->fmt.pix.height = 480;
 551                 break;
 552         case STD_PAL:
 553         case STD_SECAM:
 554                 f->fmt.pix.height = 576;
 555                 break;
 556         }
 557         f->fmt.pix.field = V4L2_FIELD_INTERLACED;
 558         f->fmt.pix.pixelformat = V4L2_PIX_FMT_H264;
 559         f->fmt.pix.sizeimage = H264_VLC_BUF_SIZE;
 560         f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 561         return 0;
 562 }
 563
 564 static int tw5864_enum_input(struct file *file, void *priv,
 565                              struct v4l2_input *i)
 566 {
 567         struct tw5864_input *input = video_drvdata(file);
 568         struct tw5864_dev *dev = input->root;
 569
 570         u8 indir_0x000 = tw_indir_readb(TW5864_INDIR_VIN_0(input->nr));
 571         u8 indir_0x00d = tw_indir_readb(TW5864_INDIR_VIN_D(input->nr));
 572         u8 v1 = indir_0x000;
 573         u8 v2 = indir_0x00d;
 574
 575         if (i->index)
 576                 return -EINVAL;
 577
 578         i->type = V4L2_INPUT_TYPE_CAMERA;
 579         snprintf(i->name, sizeof(i->name), "Encoder %d", input->nr);
 580         i->std = TW5864_NORMS;
 581         if (v1 & (1 << 7))
 582                 i->status |= V4L2_IN_ST_NO_SYNC;
 583         if (!(v1 & (1 << 6)))
 584                 i->status |= V4L2_IN_ST_NO_H_LOCK;
 585         if (v1 & (1 << 2))
 586                 i->status |= V4L2_IN_ST_NO_SIGNAL;
 587         if (v1 & (1 << 1))
 588                 i->status |= V4L2_IN_ST_NO_COLOR;
 589         if (v2 & (1 << 2))
 590                 i->status |= V4L2_IN_ST_MACROVISION;
 591
 592         return 0;
 593 }
 594
 595 static int tw5864_g_input(struct file *file, void *priv, unsigned int *i)
 596 {
 597         *i = 0;
 598         return 0;
 599 }
 600
 601 static int tw5864_s_input(struct file *file, void *priv, unsigned int i)
 602 {
 603         if (i)
 604                 return -EINVAL;
 605         return 0;
 606 }
 607
 608 static int tw5864_querycap(struct file *file, void *priv,
 609                            struct v4l2_capability *cap)
 610 {
 611         struct tw5864_input *input = video_drvdata(file);
 612
 613         strcpy(cap->driver, "tw5864");
 614         snprintf(cap->card, sizeof(cap->card), "TW5864 Encoder %d",
 615                  input->nr);
 616         sprintf(cap->bus_info, "PCI:%s", pci_name(input->root->pci));
 617         return 0;
 618 }
 619
 620 static int tw5864_querystd(struct file *file, void *priv, v4l2_std_id *std)
 621 {
 622         struct tw5864_input *input = video_drvdata(file);
 623         enum tw5864_vid_std tw_std;
 624         int ret;
 625
 626         ret = tw5864_input_std_get(input, &tw_std);
 627         if (ret)
 628                 return ret;
 629         *std = tw5864_get_v4l2_std(tw_std);
 630
 631         return 0;
 632 }
 633
 634 static int tw5864_g_std(struct file *file, void *priv, v4l2_std_id *std)
 635 {
 636         struct tw5864_input *input = video_drvdata(file);
 637
 638         *std = input->v4l2_std;
 639         return 0;
 640 }
 641
 642 static int tw5864_s_std(struct file *file, void *priv, v4l2_std_id std)
 643 {
 644         struct tw5864_input *input = video_drvdata(file);
 645         struct tw5864_dev *dev = input->root;
 646
 647         input->v4l2_std = std;
 648         input->std = tw5864_from_v4l2_std(std);
 649         tw_indir_writeb(TW5864_INDIR_VIN_E(input->nr), input->std);
 650         return 0;
 651 }
 652
 653 static int tw5864_enum_fmt_vid_cap(struct file *file, void *priv,
 654                                    struct v4l2_fmtdesc *f)
 655 {
 656         if (f->index)
 657                 return -EINVAL;
 658
 659         f->pixelformat = V4L2_PIX_FMT_H264;
 660
 661         return 0;
 662 }
 663
 664 static int tw5864_subscribe_event(struct v4l2_fh *fh,
 665                                   const struct v4l2_event_subscription *sub)
 666 {
 667         switch (sub->type) {
 668         case V4L2_EVENT_MOTION_DET:
 669                 /*
 670                  * Allow for up to 30 events (1 second for NTSC) to be stored.
 671                  */
 672                 return v4l2_event_subscribe(fh, sub, 30, NULL);
 673         default:
 674                 return v4l2_ctrl_subscribe_event(fh, sub);
 675         }
 676 }
 677
 678 static void tw5864_frame_interval_set(struct tw5864_input *input)
 679 {
 680         /*
 681          * This register value seems to follow such approach: In each second
 682          * interval, when processing Nth frame, it checks Nth bit of register
 683          * value and, if the bit is 1, it processes the frame, otherwise the
 684          * frame is discarded.
 685          * So unary representation would work, but more or less equal gaps
 686          * between the frames should be preserved.
 687          *
 688          * For 1 FPS - 0x00000001
 689          * 00000000 00000000 00000000 00000001
 690          *
 691          * For max FPS - set all 25/30 lower bits:
 692          * 00111111 11111111 11111111 11111111 (NTSC)
 693          * 00000001 11111111 11111111 11111111 (PAL)
 694          *
 695          * For half of max FPS - use such pattern:
 696          * 00010101 01010101 01010101 01010101 (NTSC)
 697          * 00000001 01010101 01010101 01010101 (PAL)
 698          *
 699          * Et cetera.
 700          *
 701          * The value supplied to hardware is capped by mask of 25/30 lower bits.
 702          */
 703         struct tw5864_dev *dev = input->root;
 704         u32 unary_framerate = 0;
 705         int shift = 0;
 706         int std_max_fps = input->std == STD_NTSC ? 30 : 25;
 707
 708         for (shift = 0; shift < std_max_fps; shift += input->frame_interval)
 709                 unary_framerate |= 0x00000001 << shift;
 710
 711         tw_writel(TW5864_H264EN_RATE_CNTL_LO_WORD(input->nr, 0),
 712                   unary_framerate >> 16);
 713         tw_writel(TW5864_H264EN_RATE_CNTL_HI_WORD(input->nr, 0),
 714                   unary_framerate & 0xffff);
 715 }
 716
 717 static int tw5864_frameinterval_get(struct tw5864_input *input,
 718                                     struct v4l2_fract *frameinterval)
 719 {
 720         struct tw5864_dev *dev = input->root;
 721
 722         switch (input->std) {
 723         case STD_NTSC:
 724                 frameinterval->numerator = 1001;
 725                 frameinterval->denominator = 30000;
 726                 break;
 727         case STD_PAL:
 728         case STD_SECAM:
 729                 frameinterval->numerator = 1;
 730                 frameinterval->denominator = 25;
 731                 break;
 732         default:
 733                 dev_warn(&dev->pci->dev, "tw5864_frameinterval_get requested for unknown std %d\n",
 734                          input->std);
 735                 return -EINVAL;
 736         }
 737
 738         return 0;
 739 }
 740
 741 static int tw5864_enum_framesizes(struct file *file, void *priv,
 742                                   struct v4l2_frmsizeenum *fsize)
 743 {
 744         struct tw5864_input *input = video_drvdata(file);
 745
 746         if (fsize->index > 0)
 747                 return -EINVAL;
 748         if (fsize->pixel_format != V4L2_PIX_FMT_H264)
 749                 return -EINVAL;
 750
 751         fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 752         fsize->discrete.width = 704;
 753         fsize->discrete.height = input->std == STD_NTSC ? 480 : 576;
 754
 755         return 0;
 756 }
 757
 758 static int tw5864_enum_frameintervals(struct file *file, void *priv,
 759                                       struct v4l2_frmivalenum *fintv)
 760 {
 761         struct tw5864_input *input = video_drvdata(file);
 762         struct v4l2_fract frameinterval;
 763         int std_max_fps = input->std == STD_NTSC ? 30 : 25;
 764         struct v4l2_frmsizeenum fsize = { .index = fintv->index,
 765                 .pixel_format = fintv->pixel_format };
 766         int ret;
 767
 768         ret = tw5864_enum_framesizes(file, priv, &fsize);
 769         if (ret)
 770                 return ret;
 771
 772         if (fintv->width != fsize.discrete.width ||
 773             fintv->height != fsize.discrete.height)
 774                 return -EINVAL;
 775
 776         fintv->type = V4L2_FRMIVAL_TYPE_STEPWISE;
 777
 778         ret = tw5864_frameinterval_get(input, &frameinterval);
 779         fintv->stepwise.step = frameinterval;
 780         fintv->stepwise.min = frameinterval;
 781         fintv->stepwise.max = frameinterval;
 782         fintv->stepwise.max.numerator *= std_max_fps;
 783
 784         return ret;
 785 }
 786
 787 static int tw5864_g_parm(struct file *file, void *priv,
 788                          struct v4l2_streamparm *sp)
 789 {
 790         struct tw5864_input *input = video_drvdata(file);
 791         struct v4l2_captureparm *cp = &sp->parm.capture;
 792         int ret;
 793
 794         cp->capability = V4L2_CAP_TIMEPERFRAME;
 795
 796         ret = tw5864_frameinterval_get(input, &cp->timeperframe);
 797         cp->timeperframe.numerator *= input->frame_interval;
 798         cp->capturemode = 0;
 799         cp->readbuffers = 2;
 800
 801         return ret;
 802 }
 803
 804 static int tw5864_s_parm(struct file *file, void *priv,
 805                          struct v4l2_streamparm *sp)
 806 {
 807         struct tw5864_input *input = video_drvdata(file);
 808         struct v4l2_fract *t = &sp->parm.capture.timeperframe;
 809         struct v4l2_fract time_base;
 810         int ret;
 811
 812         ret = tw5864_frameinterval_get(input, &time_base);
 813         if (ret)
 814                 return ret;
 815
 816         if (!t->numerator || !t->denominator) {
 817                 t->numerator = time_base.numerator * input->frame_interval;
 818                 t->denominator = time_base.denominator;
 819         } else if (t->denominator != time_base.denominator) {
 820                 t->numerator = t->numerator * time_base.denominator /
 821                         t->denominator;
 822                 t->denominator = time_base.denominator;
 823         }
 824
 825         input->frame_interval = t->numerator / time_base.numerator;
 826         if (input->frame_interval < 1)
 827                 input->frame_interval = 1;
 828         tw5864_frame_interval_set(input);
 829         return tw5864_g_parm(file, priv, sp);
 830 }
 831
 832 static const struct v4l2_ctrl_ops tw5864_ctrl_ops = {
 833         .s_ctrl = tw5864_s_ctrl,
 834 };
 835
 836 static const struct v4l2_file_operations video_fops = {
 837         .owner = THIS_MODULE,
 838         .open = v4l2_fh_open,
 839         .release = vb2_fop_release,
 840         .read = vb2_fop_read,
 841         .poll = vb2_fop_poll,
 842         .mmap = vb2_fop_mmap,
 843         .unlocked_ioctl = video_ioctl2,
 844 };
 845
 846 #ifdef CONFIG_VIDEO_ADV_DEBUG
 847
 848 #define INDIR_SPACE_MAP_SHIFT 0x100000
 849
 850 static int tw5864_g_reg(struct file *file, void *fh,
 851                         struct v4l2_dbg_register *reg)
 852 {
 853         struct tw5864_input *input = video_drvdata(file);
 854         struct tw5864_dev *dev = input->root;
 855
 856         if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
 857                 if (reg->reg > 0x87fff)
 858                         return -EINVAL;
 859                 reg->size = 4;
 860                 reg->val = tw_readl(reg->reg);
 861         } else {
 862                 __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
 863
 864                 if (indir_addr > 0xefe)
 865                         return -EINVAL;
 866                 reg->size = 1;
 867                 reg->val = tw_indir_readb(reg->reg);
 868         }
 869         return 0;
 870 }
 871
 872 static int tw5864_s_reg(struct file *file, void *fh,
 873                         const struct v4l2_dbg_register *reg)
 874 {
 875         struct tw5864_input *input = video_drvdata(file);
 876         struct tw5864_dev *dev = input->root;
 877
 878         if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
 879                 if (reg->reg > 0x87fff)
 880                         return -EINVAL;
 881                 tw_writel(reg->reg, reg->val);
 882         } else {
 883                 __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
 884
 885                 if (indir_addr > 0xefe)
 886                         return -EINVAL;
 887                 tw_indir_writeb(reg->reg, reg->val);
 888         }
 889         return 0;
 890 }
 891 #endif
 892
 893 static const struct v4l2_ioctl_ops video_ioctl_ops = {
 894         .vidioc_querycap = tw5864_querycap,
 895         .vidioc_enum_fmt_vid_cap = tw5864_enum_fmt_vid_cap,
 896         .vidioc_reqbufs = vb2_ioctl_reqbufs,
 897         .vidioc_create_bufs = vb2_ioctl_create_bufs,
 898         .vidioc_querybuf = vb2_ioctl_querybuf,
 899         .vidioc_qbuf = vb2_ioctl_qbuf,
 900         .vidioc_dqbuf = vb2_ioctl_dqbuf,
 901         .vidioc_expbuf = vb2_ioctl_expbuf,
 902         .vidioc_querystd = tw5864_querystd,
 903         .vidioc_s_std = tw5864_s_std,
 904         .vidioc_g_std = tw5864_g_std,
 905         .vidioc_enum_input = tw5864_enum_input,
 906         .vidioc_g_input = tw5864_g_input,
 907         .vidioc_s_input = tw5864_s_input,
 908         .vidioc_streamon = vb2_ioctl_streamon,
 909         .vidioc_streamoff = vb2_ioctl_streamoff,
 910         .vidioc_try_fmt_vid_cap = tw5864_fmt_vid_cap,
 911         .vidioc_s_fmt_vid_cap = tw5864_fmt_vid_cap,
 912         .vidioc_g_fmt_vid_cap = tw5864_fmt_vid_cap,
 913         .vidioc_log_status = v4l2_ctrl_log_status,
 914         .vidioc_subscribe_event = tw5864_subscribe_event,
 915         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 916         .vidioc_enum_framesizes = tw5864_enum_framesizes,
 917         .vidioc_enum_frameintervals = tw5864_enum_frameintervals,
 918         .vidioc_s_parm = tw5864_s_parm,
 919         .vidioc_g_parm = tw5864_g_parm,
 920 #ifdef CONFIG_VIDEO_ADV_DEBUG
 921         .vidioc_g_register = tw5864_g_reg,
 922         .vidioc_s_register = tw5864_s_reg,
 923 #endif
 924 };
 925
 926 static const struct video_device tw5864_video_template = {
 927         .name = "tw5864_video",
 928         .fops = &video_fops,
 929         .ioctl_ops = &video_ioctl_ops,
 930         .release = video_device_release_empty,
 931         .tvnorms = TW5864_NORMS,
 932         .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
 933                 V4L2_CAP_STREAMING,
 934 };
 935
 936 /* Motion Detection Threshold matrix */
 937 static const struct v4l2_ctrl_config tw5864_md_thresholds = {
 938         .ops = &tw5864_ctrl_ops,
 939         .id = V4L2_CID_DETECT_MD_THRESHOLD_GRID,
 940         .dims = {MD_CELLS_HOR, MD_CELLS_VERT},
 941         .def = 14,
 942         /* See tw5864_md_metric_from_mvd() */
 943         .max = 2 * 0x0f,
 944         .step = 1,
 945 };
 946
 947 static int tw5864_video_input_init(struct tw5864_input *dev, int video_nr);
 948 static void tw5864_video_input_fini(struct tw5864_input *dev);
 949 static void tw5864_encoder_tables_upload(struct tw5864_dev *dev);
 950
 951 int tw5864_video_init(struct tw5864_dev *dev, int *video_nr)
 952 {
 953         int i;
 954         int ret;
 955         unsigned long flags;
 956         int last_dma_allocated = -1;
 957         int last_input_nr_registered = -1;
 958
 959         for (i = 0; i < H264_BUF_CNT; i++) {
 960                 struct tw5864_h264_frame *frame = &dev->h264_buf[i];
 961
 962                 frame->vlc.addr = dma_alloc_coherent(&dev->pci->dev,
 963                                                      H264_VLC_BUF_SIZE,
 964                                                      &frame->vlc.dma_addr,
 965                                                      GFP_KERNEL | GFP_DMA32);
 966                 if (!frame->vlc.addr) {
 967                         dev_err(&dev->pci->dev, "dma alloc fail\n");
 968                         ret = -ENOMEM;
 969                         goto free_dma;
 970                 }
 971                 frame->mv.addr = dma_alloc_coherent(&dev->pci->dev,
 972                                                     H264_MV_BUF_SIZE,
 973                                                     &frame->mv.dma_addr,
 974                                                     GFP_KERNEL | GFP_DMA32);
 975                 if (!frame->mv.addr) {
 976                         dev_err(&dev->pci->dev, "dma alloc fail\n");
 977                         ret = -ENOMEM;
 978                         dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
 979                                           frame->vlc.addr, frame->vlc.dma_addr);
 980                         goto free_dma;
 981                 }
 982                 last_dma_allocated = i;
 983         }
 984
 985         tw5864_encoder_tables_upload(dev);
 986
 987         /* Picture is distorted without this block */
 988         /* use falling edge to sample 54M to 108M */
 989         tw_indir_writeb(TW5864_INDIR_VD_108_POL, TW5864_INDIR_VD_108_POL_BOTH);
 990         tw_indir_writeb(TW5864_INDIR_CLK0_SEL, 0x00);
 991
 992         tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL0, 0x02);
 993         tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL1, 0x02);
 994         tw_indir_writeb(TW5864_INDIR_DDRA_DLL_CLK90_SEL, 0x02);
 995         tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL0, 0x02);
 996         tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL1, 0x02);
 997         tw_indir_writeb(TW5864_INDIR_DDRB_DLL_CLK90_SEL, 0x02);
 998
 999         /* video input reset */
1000         tw_indir_writeb(TW5864_INDIR_RESET, 0);
1001         tw_indir_writeb(TW5864_INDIR_RESET, TW5864_INDIR_RESET_VD |
1002                         TW5864_INDIR_RESET_DLL | TW5864_INDIR_RESET_MUX_CORE);
1003         msleep(20);
1004
1005         /*
1006          * Select Part A mode for all channels.
1007          * tw_setl instead of tw_clearl for Part B mode.
1008          *
1009          * I guess "Part B" is primarily for downscaled version of same channel
1010          * which goes in Part A of same bus
1011          */
1012         tw_writel(TW5864_FULL_HALF_MODE_SEL, 0);
1013
1014         tw_indir_writeb(TW5864_INDIR_PV_VD_CK_POL,
1015                         TW5864_INDIR_PV_VD_CK_POL_VD(0) |
1016                         TW5864_INDIR_PV_VD_CK_POL_VD(1) |
1017                         TW5864_INDIR_PV_VD_CK_POL_VD(2) |
1018                         TW5864_INDIR_PV_VD_CK_POL_VD(3));
1019
1020         spin_lock_irqsave(&dev->slock, flags);
1021         dev->encoder_busy = 0;
1022         dev->h264_buf_r_index = 0;
1023         dev->h264_buf_w_index = 0;
1024         tw_writel(TW5864_VLC_STREAM_BASE_ADDR,
1025                   dev->h264_buf[dev->h264_buf_w_index].vlc.dma_addr);
1026         tw_writel(TW5864_MV_STREAM_BASE_ADDR,
1027                   dev->h264_buf[dev->h264_buf_w_index].mv.dma_addr);
1028         spin_unlock_irqrestore(&dev->slock, flags);
1029
1030         tw_writel(TW5864_SEN_EN_CH, 0x000f);
1031         tw_writel(TW5864_H264EN_CH_EN, 0x000f);
1032
1033         tw_writel(TW5864_H264EN_BUS0_MAP, 0x00000000);
1034         tw_writel(TW5864_H264EN_BUS1_MAP, 0x00001111);
1035         tw_writel(TW5864_H264EN_BUS2_MAP, 0x00002222);
1036         tw_writel(TW5864_H264EN_BUS3_MAP, 0x00003333);
1037
1038         /*
1039          * Quote from Intersil (manufacturer):
1040          * 0x0038 is managed by HW, and by default it won't pass the pointer set
1041          * at 0x0010. So if you don't do encoding, 0x0038 should stay at '3'
1042          * (with 4 frames in buffer). If you encode one frame and then move
1043          * 0x0010 to '1' for example, HW will take one more frame and set it to
1044          * buffer #0, and then you should see 0x0038 is set to '0'.  There is
1045          * only one HW encoder engine, so 4 channels cannot get encoded
1046          * simultaneously. But each channel does have its own buffer (for
1047          * original frames and reconstructed frames). So there is no problem to
1048          * manage encoding for 4 channels at same time and no need to force
1049          * I-frames in switching channels.
1050          * End of quote.
1051          *
1052          * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0 (for any channel), we
1053          * have no "rolling" (until we change this value).
1054          * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0x3, it starts to roll
1055          * continuously together with 0x0038.
1056          */
1057         tw_writel(TW5864_ENC_BUF_PTR_REC1, 0x00ff);
1058         tw_writel(TW5864_PCI_INTTM_SCALE, 0);
1059
1060         tw_writel(TW5864_INTERLACING, TW5864_DI_EN);
1061         tw_writel(TW5864_MASTER_ENB_REG, TW5864_PCI_VLC_INTR_ENB);
1062         tw_writel(TW5864_PCI_INTR_CTL,
1063                   TW5864_TIMER_INTR_ENB | TW5864_PCI_MAST_ENB |
1064                   TW5864_MVD_VLC_MAST_ENB);
1065
1066         dev->irqmask |= TW5864_INTR_VLC_DONE | TW5864_INTR_TIMER;
1067         tw5864_irqmask_apply(dev);
1068
1069         tasklet_init(&dev->tasklet, tw5864_handle_frame_task,
1070                      (unsigned long)dev);
1071
1072         for (i = 0; i < TW5864_INPUTS; i++) {
1073                 dev->inputs[i].root = dev;
1074                 dev->inputs[i].nr = i;
1075                 ret = tw5864_video_input_init(&dev->inputs[i], video_nr[i]);
1076                 if (ret)
1077                         goto fini_video_inputs;
1078                 last_input_nr_registered = i;
1079         }
1080
1081         return 0;
1082
1083 fini_video_inputs:
1084         for (i = last_input_nr_registered; i >= 0; i--)
1085                 tw5864_video_input_fini(&dev->inputs[i]);
1086
1087         tasklet_kill(&dev->tasklet);
1088
1089 free_dma:
1090         for (i = last_dma_allocated; i >= 0; i--) {
1091                 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1092                                   dev->h264_buf[i].vlc.addr,
1093                                   dev->h264_buf[i].vlc.dma_addr);
1094                 dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1095                                   dev->h264_buf[i].mv.addr,
1096                                   dev->h264_buf[i].mv.dma_addr);
1097         }
1098
1099         return ret;
1100 }
1101
1102 static int tw5864_video_input_init(struct tw5864_input *input, int video_nr)
1103 {
1104         struct tw5864_dev *dev = input->root;
1105         int ret;
1106         struct v4l2_ctrl_handler *hdl = &input->hdl;
1107
1108         mutex_init(&input->lock);
1109         spin_lock_init(&input->slock);
1110
1111         /* setup video buffers queue */
1112         INIT_LIST_HEAD(&input->active);
1113         input->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1114         input->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1115         input->vidq.io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
1116         input->vidq.ops = &tw5864_video_qops;
1117         input->vidq.mem_ops = &vb2_dma_contig_memops;
1118         input->vidq.drv_priv = input;
1119         input->vidq.gfp_flags = 0;
1120         input->vidq.buf_struct_size = sizeof(struct tw5864_buf);
1121         input->vidq.lock = &input->lock;
1122         input->vidq.min_buffers_needed = 2;
1123         input->vidq.dev = &input->root->pci->dev;
1124         ret = vb2_queue_init(&input->vidq);
1125         if (ret)
1126                 goto free_mutex;
1127
1128         input->vdev = tw5864_video_template;
1129         input->vdev.v4l2_dev = &input->root->v4l2_dev;
1130         input->vdev.lock = &input->lock;
1131         input->vdev.queue = &input->vidq;
1132         video_set_drvdata(&input->vdev, input);
1133
1134         /* Initialize the device control structures */
1135         v4l2_ctrl_handler_init(hdl, 6);
1136         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1137                           V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1138         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1139                           V4L2_CID_CONTRAST, 0, 255, 1, 100);
1140         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1141                           V4L2_CID_SATURATION, 0, 255, 1, 128);
1142         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_HUE, -128, 127, 1, 0);
1143         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1144                           1, MAX_GOP_SIZE, 1, GOP_SIZE);
1145         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1146                           V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 28, 51, 1, QP_VALUE);
1147         v4l2_ctrl_new_std_menu(hdl, &tw5864_ctrl_ops,
1148                                V4L2_CID_DETECT_MD_MODE,
1149                                V4L2_DETECT_MD_MODE_THRESHOLD_GRID, 0,
1150                                V4L2_DETECT_MD_MODE_DISABLED);
1151         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1152                           V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD,
1153                           tw5864_md_thresholds.min, tw5864_md_thresholds.max,
1154                           tw5864_md_thresholds.step, tw5864_md_thresholds.def);
1155         input->md_threshold_grid_ctrl =
1156                 v4l2_ctrl_new_custom(hdl, &tw5864_md_thresholds, NULL);
1157         if (hdl->error) {
1158                 ret = hdl->error;
1159                 goto free_v4l2_hdl;
1160         }
1161         input->vdev.ctrl_handler = hdl;
1162         v4l2_ctrl_handler_setup(hdl);
1163
1164         input->qp = QP_VALUE;
1165         input->gop = GOP_SIZE;
1166         input->frame_interval = 1;
1167
1168         ret = video_register_device(&input->vdev, VFL_TYPE_GRABBER, video_nr);
1169         if (ret)
1170                 goto free_v4l2_hdl;
1171
1172         dev_info(&input->root->pci->dev, "Registered video device %s\n",
1173                  video_device_node_name(&input->vdev));
1174
1175         /*
1176          * Set default video standard. Doesn't matter which, the detected value
1177          * will be found out by VIDIOC_QUERYSTD handler.
1178          */
1179         input->v4l2_std = V4L2_STD_NTSC_M;
1180         input->std = STD_NTSC;
1181
1182         tw_indir_writeb(TW5864_INDIR_VIN_E(video_nr), 0x07);
1183         /* to initiate auto format recognition */
1184         tw_indir_writeb(TW5864_INDIR_VIN_F(video_nr), 0xff);
1185
1186         return 0;
1187
1188 free_v4l2_hdl:
1189         v4l2_ctrl_handler_free(hdl);
1190         vb2_queue_release(&input->vidq);
1191 free_mutex:
1192         mutex_destroy(&input->lock);
1193
1194         return ret;
1195 }
1196
1197 static void tw5864_video_input_fini(struct tw5864_input *dev)
1198 {
1199         video_unregister_device(&dev->vdev);
1200         v4l2_ctrl_handler_free(&dev->hdl);
1201         vb2_queue_release(&dev->vidq);
1202 }
1203
1204 void tw5864_video_fini(struct tw5864_dev *dev)
1205 {
1206         int i;
1207
1208         tasklet_kill(&dev->tasklet);
1209
1210         for (i = 0; i < TW5864_INPUTS; i++)
1211                 tw5864_video_input_fini(&dev->inputs[i]);
1212
1213         for (i = 0; i < H264_BUF_CNT; i++) {
1214                 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1215                                   dev->h264_buf[i].vlc.addr,
1216                                   dev->h264_buf[i].vlc.dma_addr);
1217                 dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1218                                   dev->h264_buf[i].mv.addr,
1219                                   dev->h264_buf[i].mv.dma_addr);
1220         }
1221 }
1222
1223 void tw5864_prepare_frame_headers(struct tw5864_input *input)
1224 {
1225         struct tw5864_buf *vb = input->vb;
1226         u8 *dst;
1227         size_t dst_space;
1228         unsigned long flags;
1229
1230         if (!vb) {
1231                 spin_lock_irqsave(&input->slock, flags);
1232                 if (list_empty(&input->active)) {
1233                         spin_unlock_irqrestore(&input->slock, flags);
1234                         input->vb = NULL;
1235                         return;
1236                 }
1237                 vb = list_first_entry(&input->active, struct tw5864_buf, list);
1238                 list_del(&vb->list);
1239                 spin_unlock_irqrestore(&input->slock, flags);
1240         }
1241
1242         dst = vb2_plane_vaddr(&vb->vb.vb2_buf, 0);
1243         dst_space = vb2_plane_size(&vb->vb.vb2_buf, 0);
1244
1245         /*
1246          * Low-level bitstream writing functions don't have a fine way to say
1247          * correctly that supplied buffer is too small. So we just check there
1248          * and warn, and don't care at lower level.
1249          * Currently all headers take below 32 bytes.
1250          * The buffer is supposed to have plenty of free space at this point,
1251          * anyway.
1252          */
1253         if (WARN_ON_ONCE(dst_space < 128))
1254                 return;
1255
1256         /*
1257          * Generate H264 headers:
1258          * If this is first frame, put SPS and PPS
1259          */
1260         if (input->frame_gop_seqno == 0)
1261                 tw5864_h264_put_stream_header(&dst, &dst_space, input->qp,
1262                                               input->width, input->height);
1263
1264         /* Put slice header */
1265         tw5864_h264_put_slice_header(&dst, &dst_space, input->h264_idr_pic_id,
1266                                      input->frame_gop_seqno,
1267                                      &input->tail_nb_bits, &input->tail);
1268         input->vb = vb;
1269         input->buf_cur_ptr = dst;
1270         input->buf_cur_space_left = dst_space;
1271 }
1272
1273 /*
1274  * Returns heuristic motion detection metric value from known components of
1275  * hardware-provided Motion Vector Data.
1276  */
1277 static unsigned int tw5864_md_metric_from_mvd(u32 mvd)
1278 {
1279         /*
1280          * Format of motion vector data exposed by tw5864, according to
1281          * manufacturer:
1282          * mv_x 10 bits
1283          * mv_y 10 bits
1284          * non_zero_members 8 bits
1285          * mb_type 3 bits
1286          * reserved 1 bit
1287          *
1288          * non_zero_members: number of non-zero residuals in each macro block
1289          * after quantization
1290          *
1291          * unsigned int reserved = mvd >> 31;
1292          * unsigned int mb_type = (mvd >> 28) & 0x7;
1293          * unsigned int non_zero_members = (mvd >> 20) & 0xff;
1294          */
1295         unsigned int mv_y = (mvd >> 10) & 0x3ff;
1296         unsigned int mv_x = mvd & 0x3ff;
1297
1298         /* heuristic: */
1299         mv_x &= 0x0f;
1300         mv_y &= 0x0f;
1301
1302         return mv_y + mv_x;
1303 }
1304
1305 static int tw5864_is_motion_triggered(struct tw5864_h264_frame *frame)
1306 {
1307         struct tw5864_input *input = frame->input;
1308         u32 *mv = (u32 *)frame->mv.addr;
1309         int i;
1310         int detected = 0;
1311
1312         for (i = 0; i < MD_CELLS; i++) {
1313                 const u16 thresh = input->md_threshold_grid_values[i];
1314                 const unsigned int metric = tw5864_md_metric_from_mvd(mv[i]);
1315
1316                 if (metric > thresh)
1317                         detected = 1;
1318
1319                 if (detected)
1320                         break;
1321         }
1322         return detected;
1323 }
1324
1325 static void tw5864_handle_frame_task(unsigned long data)
1326 {
1327         struct tw5864_dev *dev = (struct tw5864_dev *)data;
1328         unsigned long flags;
1329         int batch_size = H264_BUF_CNT;
1330
1331         spin_lock_irqsave(&dev->slock, flags);
1332         while (dev->h264_buf_r_index != dev->h264_buf_w_index && batch_size--) {
1333                 struct tw5864_h264_frame *frame =
1334                         &dev->h264_buf[dev->h264_buf_r_index];
1335
1336                 spin_unlock_irqrestore(&dev->slock, flags);
1337                 dma_sync_single_for_cpu(&dev->pci->dev, frame->vlc.dma_addr,
1338                                         H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1339                 dma_sync_single_for_cpu(&dev->pci->dev, frame->mv.dma_addr,
1340                                         H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1341                 tw5864_handle_frame(frame);
1342                 dma_sync_single_for_device(&dev->pci->dev, frame->vlc.dma_addr,
1343                                            H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1344                 dma_sync_single_for_device(&dev->pci->dev, frame->mv.dma_addr,
1345                                            H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1346                 spin_lock_irqsave(&dev->slock, flags);
1347
1348                 dev->h264_buf_r_index++;
1349                 dev->h264_buf_r_index %= H264_BUF_CNT;
1350         }
1351         spin_unlock_irqrestore(&dev->slock, flags);
1352 }
1353
1354 #ifdef DEBUG
1355 static u32 tw5864_vlc_checksum(u32 *data, int len)
1356 {
1357         u32 val, count_len = len;
1358
1359         val = *data++;
1360         while (((count_len >> 2) - 1) > 0) {
1361                 val ^= *data++;
1362                 count_len -= 4;
1363         }
1364         val ^= htonl((len >> 2));
1365         return val;
1366 }
1367 #endif
1368
1369 static void tw5864_handle_frame(struct tw5864_h264_frame *frame)
1370 {
1371 #define SKIP_VLCBUF_BYTES 3
1372         struct tw5864_input *input = frame->input;
1373         struct tw5864_dev *dev = input->root;
1374         struct tw5864_buf *vb;
1375         struct vb2_v4l2_buffer *v4l2_buf;
1376         int frame_len = frame->vlc_len - SKIP_VLCBUF_BYTES;
1377         u8 *dst = input->buf_cur_ptr;
1378         u8 tail_mask, vlc_mask = 0;
1379         int i;
1380         u8 vlc_first_byte = ((u8 *)(frame->vlc.addr + SKIP_VLCBUF_BYTES))[0];
1381         unsigned long flags;
1382         int zero_run;
1383         u8 *src;
1384         u8 *src_end;
1385
1386 #ifdef DEBUG
1387         if (frame->checksum !=
1388             tw5864_vlc_checksum((u32 *)frame->vlc.addr, frame_len))
1389                 dev_err(&dev->pci->dev,
1390                         "Checksum of encoded frame doesn't match!\n");
1391 #endif
1392
1393         spin_lock_irqsave(&input->slock, flags);
1394         vb = input->vb;
1395         input->vb = NULL;
1396         spin_unlock_irqrestore(&input->slock, flags);
1397
1398         v4l2_buf = to_vb2_v4l2_buffer(&vb->vb.vb2_buf);
1399
1400         if (!vb) { /* Gone because of disabling */
1401                 dev_dbg(&dev->pci->dev, "vb is empty, dropping frame\n");
1402                 return;
1403         }
1404
1405         /*
1406          * Check for space.
1407          * Mind the overhead of startcode emulation prevention.
1408          */
1409         if (input->buf_cur_space_left < frame_len * 5 / 4) {
1410                 dev_err_once(&dev->pci->dev,
1411                              "Left space in vb2 buffer, %d bytes, is less than considered safely enough to put frame of length %d. Dropping this frame.\n",
1412                              input->buf_cur_space_left, frame_len);
1413                 return;
1414         }
1415
1416         for (i = 0; i < 8 - input->tail_nb_bits; i++)
1417                 vlc_mask |= 1 << i;
1418         tail_mask = (~vlc_mask) & 0xff;
1419
1420         dst[0] = (input->tail & tail_mask) | (vlc_first_byte & vlc_mask);
1421         frame_len--;
1422         dst++;
1423
1424         /* H.264 startcode emulation prevention */
1425         src = frame->vlc.addr + SKIP_VLCBUF_BYTES + 1;
1426         src_end = src + frame_len;
1427         zero_run = 0;
1428         for (; src < src_end; src++) {
1429                 if (zero_run < 2) {
1430                         if (*src == 0)
1431                                 ++zero_run;
1432                         else
1433                                 zero_run = 0;
1434                 } else {
1435                         if ((*src & ~0x03) == 0)
1436                                 *dst++ = 0x03;
1437                         zero_run = *src == 0;
1438                 }
1439                 *dst++ = *src;
1440         }
1441
1442         vb2_set_plane_payload(&vb->vb.vb2_buf, 0,
1443                               dst - (u8 *)vb2_plane_vaddr(&vb->vb.vb2_buf, 0));
1444
1445         vb->vb.vb2_buf.timestamp = frame->timestamp;
1446         v4l2_buf->field = V4L2_FIELD_INTERLACED;
1447         v4l2_buf->sequence = frame->seqno;
1448
1449         /* Check for motion flags */
1450         if (frame->gop_seqno /* P-frame */ &&
1451             tw5864_is_motion_triggered(frame)) {
1452                 struct v4l2_event ev = {
1453                         .type = V4L2_EVENT_MOTION_DET,
1454                         .u.motion_det = {
1455                                 .flags = V4L2_EVENT_MD_FL_HAVE_FRAME_SEQ,
1456                                 .frame_sequence = v4l2_buf->sequence,
1457                         },
1458                 };
1459
1460                 v4l2_event_queue(&input->vdev, &ev);
1461         }
1462
1463         vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
1464 }
1465
1466 static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std std)
1467 {
1468         switch (std) {
1469         case STD_NTSC:    return V4L2_STD_NTSC_M;
1470         case STD_PAL:     return V4L2_STD_PAL_B;
1471         case STD_SECAM:   return V4L2_STD_SECAM_B;
1472         case STD_NTSC443: return V4L2_STD_NTSC_443;
1473         case STD_PAL_M:   return V4L2_STD_PAL_M;
1474         case STD_PAL_CN:  return V4L2_STD_PAL_Nc;
1475         case STD_PAL_60:  return V4L2_STD_PAL_60;
1476         case STD_INVALID: return V4L2_STD_UNKNOWN;
1477         }
1478         return 0;
1479 }
1480
1481 static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std)
1482 {
1483         if (v4l2_std & V4L2_STD_NTSC_M)
1484                 return STD_NTSC;
1485         if (v4l2_std & V4L2_STD_PAL_B)
1486                 return STD_PAL;
1487         if (v4l2_std & V4L2_STD_SECAM_B)
1488                 return STD_SECAM;
1489         if (v4l2_std & V4L2_STD_NTSC_443)
1490                 return STD_NTSC443;
1491         if (v4l2_std & V4L2_STD_PAL_M)
1492                 return STD_PAL_M;
1493         if (v4l2_std & V4L2_STD_PAL_Nc)
1494                 return STD_PAL_CN;
1495         if (v4l2_std & V4L2_STD_PAL_60)
1496                 return STD_PAL_60;
1497
1498         return STD_INVALID;
1499 }
1500
1501 static void tw5864_encoder_tables_upload(struct tw5864_dev *dev)
1502 {
1503         int i;
1504
1505         tw_writel(TW5864_VLC_RD, 0x1);
1506         for (i = 0; i < VLC_LOOKUP_TABLE_LEN; i++) {
1507                 tw_writel((TW5864_VLC_STREAM_MEM_START + i * 4),
1508                           encoder_vlc_lookup_table[i]);
1509         }
1510         tw_writel(TW5864_VLC_RD, 0x0);
1511
1512         for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1513                 tw_writel((TW5864_QUAN_TAB + i * 4),
1514                           forward_quantization_table[i]);
1515         }
1516
1517         for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1518                 tw_writel((TW5864_QUAN_TAB + i * 4),
1519                           inverse_quantization_table[i]);
1520         }
1521 }