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         if (ret)
 780                 return ret;
 781
 782         fintv->stepwise.step = frameinterval;
 783         fintv->stepwise.min = frameinterval;
 784         fintv->stepwise.max = frameinterval;
 785         fintv->stepwise.max.numerator *= std_max_fps;
 786
 787         return ret;
 788 }
 789
 790 static int tw5864_g_parm(struct file *file, void *priv,
 791                          struct v4l2_streamparm *sp)
 792 {
 793         struct tw5864_input *input = video_drvdata(file);
 794         struct v4l2_captureparm *cp = &sp->parm.capture;
 795         int ret;
 796
 797         cp->capability = V4L2_CAP_TIMEPERFRAME;
 798
 799         ret = tw5864_frameinterval_get(input, &cp->timeperframe);
 800         if (ret)
 801                 return ret;
 802
 803         cp->timeperframe.numerator *= input->frame_interval;
 804         cp->capturemode = 0;
 805         cp->readbuffers = 2;
 806
 807         return ret;
 808 }
 809
 810 static int tw5864_s_parm(struct file *file, void *priv,
 811                          struct v4l2_streamparm *sp)
 812 {
 813         struct tw5864_input *input = video_drvdata(file);
 814         struct v4l2_fract *t = &sp->parm.capture.timeperframe;
 815         struct v4l2_fract time_base;
 816         int ret;
 817
 818         ret = tw5864_frameinterval_get(input, &time_base);
 819         if (ret)
 820                 return ret;
 821
 822         if (!t->numerator || !t->denominator) {
 823                 t->numerator = time_base.numerator * input->frame_interval;
 824                 t->denominator = time_base.denominator;
 825         } else if (t->denominator != time_base.denominator) {
 826                 t->numerator = t->numerator * time_base.denominator /
 827                         t->denominator;
 828                 t->denominator = time_base.denominator;
 829         }
 830
 831         input->frame_interval = t->numerator / time_base.numerator;
 832         if (input->frame_interval < 1)
 833                 input->frame_interval = 1;
 834         tw5864_frame_interval_set(input);
 835         return tw5864_g_parm(file, priv, sp);
 836 }
 837
 838 static const struct v4l2_ctrl_ops tw5864_ctrl_ops = {
 839         .s_ctrl = tw5864_s_ctrl,
 840 };
 841
 842 static const struct v4l2_file_operations video_fops = {
 843         .owner = THIS_MODULE,
 844         .open = v4l2_fh_open,
 845         .release = vb2_fop_release,
 846         .read = vb2_fop_read,
 847         .poll = vb2_fop_poll,
 848         .mmap = vb2_fop_mmap,
 849         .unlocked_ioctl = video_ioctl2,
 850 };
 851
 852 #ifdef CONFIG_VIDEO_ADV_DEBUG
 853
 854 #define INDIR_SPACE_MAP_SHIFT 0x100000
 855
 856 static int tw5864_g_reg(struct file *file, void *fh,
 857                         struct v4l2_dbg_register *reg)
 858 {
 859         struct tw5864_input *input = video_drvdata(file);
 860         struct tw5864_dev *dev = input->root;
 861
 862         if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
 863                 if (reg->reg > 0x87fff)
 864                         return -EINVAL;
 865                 reg->size = 4;
 866                 reg->val = tw_readl(reg->reg);
 867         } else {
 868                 __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
 869
 870                 if (indir_addr > 0xefe)
 871                         return -EINVAL;
 872                 reg->size = 1;
 873                 reg->val = tw_indir_readb(reg->reg);
 874         }
 875         return 0;
 876 }
 877
 878 static int tw5864_s_reg(struct file *file, void *fh,
 879                         const struct v4l2_dbg_register *reg)
 880 {
 881         struct tw5864_input *input = video_drvdata(file);
 882         struct tw5864_dev *dev = input->root;
 883
 884         if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
 885                 if (reg->reg > 0x87fff)
 886                         return -EINVAL;
 887                 tw_writel(reg->reg, reg->val);
 888         } else {
 889                 __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
 890
 891                 if (indir_addr > 0xefe)
 892                         return -EINVAL;
 893                 tw_indir_writeb(reg->reg, reg->val);
 894         }
 895         return 0;
 896 }
 897 #endif
 898
 899 static const struct v4l2_ioctl_ops video_ioctl_ops = {
 900         .vidioc_querycap = tw5864_querycap,
 901         .vidioc_enum_fmt_vid_cap = tw5864_enum_fmt_vid_cap,
 902         .vidioc_reqbufs = vb2_ioctl_reqbufs,
 903         .vidioc_create_bufs = vb2_ioctl_create_bufs,
 904         .vidioc_querybuf = vb2_ioctl_querybuf,
 905         .vidioc_qbuf = vb2_ioctl_qbuf,
 906         .vidioc_dqbuf = vb2_ioctl_dqbuf,
 907         .vidioc_expbuf = vb2_ioctl_expbuf,
 908         .vidioc_querystd = tw5864_querystd,
 909         .vidioc_s_std = tw5864_s_std,
 910         .vidioc_g_std = tw5864_g_std,
 911         .vidioc_enum_input = tw5864_enum_input,
 912         .vidioc_g_input = tw5864_g_input,
 913         .vidioc_s_input = tw5864_s_input,
 914         .vidioc_streamon = vb2_ioctl_streamon,
 915         .vidioc_streamoff = vb2_ioctl_streamoff,
 916         .vidioc_try_fmt_vid_cap = tw5864_fmt_vid_cap,
 917         .vidioc_s_fmt_vid_cap = tw5864_fmt_vid_cap,
 918         .vidioc_g_fmt_vid_cap = tw5864_fmt_vid_cap,
 919         .vidioc_log_status = v4l2_ctrl_log_status,
 920         .vidioc_subscribe_event = tw5864_subscribe_event,
 921         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 922         .vidioc_enum_framesizes = tw5864_enum_framesizes,
 923         .vidioc_enum_frameintervals = tw5864_enum_frameintervals,
 924         .vidioc_s_parm = tw5864_s_parm,
 925         .vidioc_g_parm = tw5864_g_parm,
 926 #ifdef CONFIG_VIDEO_ADV_DEBUG
 927         .vidioc_g_register = tw5864_g_reg,
 928         .vidioc_s_register = tw5864_s_reg,
 929 #endif
 930 };
 931
 932 static const struct video_device tw5864_video_template = {
 933         .name = "tw5864_video",
 934         .fops = &video_fops,
 935         .ioctl_ops = &video_ioctl_ops,
 936         .release = video_device_release_empty,
 937         .tvnorms = TW5864_NORMS,
 938         .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
 939                 V4L2_CAP_STREAMING,
 940 };
 941
 942 /* Motion Detection Threshold matrix */
 943 static const struct v4l2_ctrl_config tw5864_md_thresholds = {
 944         .ops = &tw5864_ctrl_ops,
 945         .id = V4L2_CID_DETECT_MD_THRESHOLD_GRID,
 946         .dims = {MD_CELLS_HOR, MD_CELLS_VERT},
 947         .def = 14,
 948         /* See tw5864_md_metric_from_mvd() */
 949         .max = 2 * 0x0f,
 950         .step = 1,
 951 };
 952
 953 static int tw5864_video_input_init(struct tw5864_input *dev, int video_nr);
 954 static void tw5864_video_input_fini(struct tw5864_input *dev);
 955 static void tw5864_encoder_tables_upload(struct tw5864_dev *dev);
 956
 957 int tw5864_video_init(struct tw5864_dev *dev, int *video_nr)
 958 {
 959         int i;
 960         int ret;
 961         unsigned long flags;
 962         int last_dma_allocated = -1;
 963         int last_input_nr_registered = -1;
 964
 965         for (i = 0; i < H264_BUF_CNT; i++) {
 966                 struct tw5864_h264_frame *frame = &dev->h264_buf[i];
 967
 968                 frame->vlc.addr = dma_alloc_coherent(&dev->pci->dev,
 969                                                      H264_VLC_BUF_SIZE,
 970                                                      &frame->vlc.dma_addr,
 971                                                      GFP_KERNEL | GFP_DMA32);
 972                 if (!frame->vlc.addr) {
 973                         dev_err(&dev->pci->dev, "dma alloc fail\n");
 974                         ret = -ENOMEM;
 975                         goto free_dma;
 976                 }
 977                 frame->mv.addr = dma_alloc_coherent(&dev->pci->dev,
 978                                                     H264_MV_BUF_SIZE,
 979                                                     &frame->mv.dma_addr,
 980                                                     GFP_KERNEL | GFP_DMA32);
 981                 if (!frame->mv.addr) {
 982                         dev_err(&dev->pci->dev, "dma alloc fail\n");
 983                         ret = -ENOMEM;
 984                         dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
 985                                           frame->vlc.addr, frame->vlc.dma_addr);
 986                         goto free_dma;
 987                 }
 988                 last_dma_allocated = i;
 989         }
 990
 991         tw5864_encoder_tables_upload(dev);
 992
 993         /* Picture is distorted without this block */
 994         /* use falling edge to sample 54M to 108M */
 995         tw_indir_writeb(TW5864_INDIR_VD_108_POL, TW5864_INDIR_VD_108_POL_BOTH);
 996         tw_indir_writeb(TW5864_INDIR_CLK0_SEL, 0x00);
 997
 998         tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL0, 0x02);
 999         tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL1, 0x02);
1000         tw_indir_writeb(TW5864_INDIR_DDRA_DLL_CLK90_SEL, 0x02);
1001         tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL0, 0x02);
1002         tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL1, 0x02);
1003         tw_indir_writeb(TW5864_INDIR_DDRB_DLL_CLK90_SEL, 0x02);
1004
1005         /* video input reset */
1006         tw_indir_writeb(TW5864_INDIR_RESET, 0);
1007         tw_indir_writeb(TW5864_INDIR_RESET, TW5864_INDIR_RESET_VD |
1008                         TW5864_INDIR_RESET_DLL | TW5864_INDIR_RESET_MUX_CORE);
1009         msleep(20);
1010
1011         /*
1012          * Select Part A mode for all channels.
1013          * tw_setl instead of tw_clearl for Part B mode.
1014          *
1015          * I guess "Part B" is primarily for downscaled version of same channel
1016          * which goes in Part A of same bus
1017          */
1018         tw_writel(TW5864_FULL_HALF_MODE_SEL, 0);
1019
1020         tw_indir_writeb(TW5864_INDIR_PV_VD_CK_POL,
1021                         TW5864_INDIR_PV_VD_CK_POL_VD(0) |
1022                         TW5864_INDIR_PV_VD_CK_POL_VD(1) |
1023                         TW5864_INDIR_PV_VD_CK_POL_VD(2) |
1024                         TW5864_INDIR_PV_VD_CK_POL_VD(3));
1025
1026         spin_lock_irqsave(&dev->slock, flags);
1027         dev->encoder_busy = 0;
1028         dev->h264_buf_r_index = 0;
1029         dev->h264_buf_w_index = 0;
1030         tw_writel(TW5864_VLC_STREAM_BASE_ADDR,
1031                   dev->h264_buf[dev->h264_buf_w_index].vlc.dma_addr);
1032         tw_writel(TW5864_MV_STREAM_BASE_ADDR,
1033                   dev->h264_buf[dev->h264_buf_w_index].mv.dma_addr);
1034         spin_unlock_irqrestore(&dev->slock, flags);
1035
1036         tw_writel(TW5864_SEN_EN_CH, 0x000f);
1037         tw_writel(TW5864_H264EN_CH_EN, 0x000f);
1038
1039         tw_writel(TW5864_H264EN_BUS0_MAP, 0x00000000);
1040         tw_writel(TW5864_H264EN_BUS1_MAP, 0x00001111);
1041         tw_writel(TW5864_H264EN_BUS2_MAP, 0x00002222);
1042         tw_writel(TW5864_H264EN_BUS3_MAP, 0x00003333);
1043
1044         /*
1045          * Quote from Intersil (manufacturer):
1046          * 0x0038 is managed by HW, and by default it won't pass the pointer set
1047          * at 0x0010. So if you don't do encoding, 0x0038 should stay at '3'
1048          * (with 4 frames in buffer). If you encode one frame and then move
1049          * 0x0010 to '1' for example, HW will take one more frame and set it to
1050          * buffer #0, and then you should see 0x0038 is set to '0'.  There is
1051          * only one HW encoder engine, so 4 channels cannot get encoded
1052          * simultaneously. But each channel does have its own buffer (for
1053          * original frames and reconstructed frames). So there is no problem to
1054          * manage encoding for 4 channels at same time and no need to force
1055          * I-frames in switching channels.
1056          * End of quote.
1057          *
1058          * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0 (for any channel), we
1059          * have no "rolling" (until we change this value).
1060          * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0x3, it starts to roll
1061          * continuously together with 0x0038.
1062          */
1063         tw_writel(TW5864_ENC_BUF_PTR_REC1, 0x00ff);
1064         tw_writel(TW5864_PCI_INTTM_SCALE, 0);
1065
1066         tw_writel(TW5864_INTERLACING, TW5864_DI_EN);
1067         tw_writel(TW5864_MASTER_ENB_REG, TW5864_PCI_VLC_INTR_ENB);
1068         tw_writel(TW5864_PCI_INTR_CTL,
1069                   TW5864_TIMER_INTR_ENB | TW5864_PCI_MAST_ENB |
1070                   TW5864_MVD_VLC_MAST_ENB);
1071
1072         dev->irqmask |= TW5864_INTR_VLC_DONE | TW5864_INTR_TIMER;
1073         tw5864_irqmask_apply(dev);
1074
1075         tasklet_init(&dev->tasklet, tw5864_handle_frame_task,
1076                      (unsigned long)dev);
1077
1078         for (i = 0; i < TW5864_INPUTS; i++) {
1079                 dev->inputs[i].root = dev;
1080                 dev->inputs[i].nr = i;
1081                 ret = tw5864_video_input_init(&dev->inputs[i], video_nr[i]);
1082                 if (ret)
1083                         goto fini_video_inputs;
1084                 last_input_nr_registered = i;
1085         }
1086
1087         return 0;
1088
1089 fini_video_inputs:
1090         for (i = last_input_nr_registered; i >= 0; i--)
1091                 tw5864_video_input_fini(&dev->inputs[i]);
1092
1093         tasklet_kill(&dev->tasklet);
1094
1095 free_dma:
1096         for (i = last_dma_allocated; i >= 0; i--) {
1097                 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1098                                   dev->h264_buf[i].vlc.addr,
1099                                   dev->h264_buf[i].vlc.dma_addr);
1100                 dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1101                                   dev->h264_buf[i].mv.addr,
1102                                   dev->h264_buf[i].mv.dma_addr);
1103         }
1104
1105         return ret;
1106 }
1107
1108 static int tw5864_video_input_init(struct tw5864_input *input, int video_nr)
1109 {
1110         struct tw5864_dev *dev = input->root;
1111         int ret;
1112         struct v4l2_ctrl_handler *hdl = &input->hdl;
1113
1114         mutex_init(&input->lock);
1115         spin_lock_init(&input->slock);
1116
1117         /* setup video buffers queue */
1118         INIT_LIST_HEAD(&input->active);
1119         input->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1120         input->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1121         input->vidq.io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
1122         input->vidq.ops = &tw5864_video_qops;
1123         input->vidq.mem_ops = &vb2_dma_contig_memops;
1124         input->vidq.drv_priv = input;
1125         input->vidq.gfp_flags = 0;
1126         input->vidq.buf_struct_size = sizeof(struct tw5864_buf);
1127         input->vidq.lock = &input->lock;
1128         input->vidq.min_buffers_needed = 2;
1129         input->vidq.dev = &input->root->pci->dev;
1130         ret = vb2_queue_init(&input->vidq);
1131         if (ret)
1132                 goto free_mutex;
1133
1134         input->vdev = tw5864_video_template;
1135         input->vdev.v4l2_dev = &input->root->v4l2_dev;
1136         input->vdev.lock = &input->lock;
1137         input->vdev.queue = &input->vidq;
1138         video_set_drvdata(&input->vdev, input);
1139
1140         /* Initialize the device control structures */
1141         v4l2_ctrl_handler_init(hdl, 6);
1142         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1143                           V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1144         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1145                           V4L2_CID_CONTRAST, 0, 255, 1, 100);
1146         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1147                           V4L2_CID_SATURATION, 0, 255, 1, 128);
1148         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_HUE, -128, 127, 1, 0);
1149         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1150                           1, MAX_GOP_SIZE, 1, GOP_SIZE);
1151         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1152                           V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 28, 51, 1, QP_VALUE);
1153         v4l2_ctrl_new_std_menu(hdl, &tw5864_ctrl_ops,
1154                                V4L2_CID_DETECT_MD_MODE,
1155                                V4L2_DETECT_MD_MODE_THRESHOLD_GRID, 0,
1156                                V4L2_DETECT_MD_MODE_DISABLED);
1157         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1158                           V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD,
1159                           tw5864_md_thresholds.min, tw5864_md_thresholds.max,
1160                           tw5864_md_thresholds.step, tw5864_md_thresholds.def);
1161         input->md_threshold_grid_ctrl =
1162                 v4l2_ctrl_new_custom(hdl, &tw5864_md_thresholds, NULL);
1163         if (hdl->error) {
1164                 ret = hdl->error;
1165                 goto free_v4l2_hdl;
1166         }
1167         input->vdev.ctrl_handler = hdl;
1168         v4l2_ctrl_handler_setup(hdl);
1169
1170         input->qp = QP_VALUE;
1171         input->gop = GOP_SIZE;
1172         input->frame_interval = 1;
1173
1174         ret = video_register_device(&input->vdev, VFL_TYPE_GRABBER, video_nr);
1175         if (ret)
1176                 goto free_v4l2_hdl;
1177
1178         dev_info(&input->root->pci->dev, "Registered video device %s\n",
1179                  video_device_node_name(&input->vdev));
1180
1181         /*
1182          * Set default video standard. Doesn't matter which, the detected value
1183          * will be found out by VIDIOC_QUERYSTD handler.
1184          */
1185         input->v4l2_std = V4L2_STD_NTSC_M;
1186         input->std = STD_NTSC;
1187
1188         tw_indir_writeb(TW5864_INDIR_VIN_E(video_nr), 0x07);
1189         /* to initiate auto format recognition */
1190         tw_indir_writeb(TW5864_INDIR_VIN_F(video_nr), 0xff);
1191
1192         return 0;
1193
1194 free_v4l2_hdl:
1195         v4l2_ctrl_handler_free(hdl);
1196         vb2_queue_release(&input->vidq);
1197 free_mutex:
1198         mutex_destroy(&input->lock);
1199
1200         return ret;
1201 }
1202
1203 static void tw5864_video_input_fini(struct tw5864_input *dev)
1204 {
1205         video_unregister_device(&dev->vdev);
1206         v4l2_ctrl_handler_free(&dev->hdl);
1207         vb2_queue_release(&dev->vidq);
1208 }
1209
1210 void tw5864_video_fini(struct tw5864_dev *dev)
1211 {
1212         int i;
1213
1214         tasklet_kill(&dev->tasklet);
1215
1216         for (i = 0; i < TW5864_INPUTS; i++)
1217                 tw5864_video_input_fini(&dev->inputs[i]);
1218
1219         for (i = 0; i < H264_BUF_CNT; i++) {
1220                 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1221                                   dev->h264_buf[i].vlc.addr,
1222                                   dev->h264_buf[i].vlc.dma_addr);
1223                 dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1224                                   dev->h264_buf[i].mv.addr,
1225                                   dev->h264_buf[i].mv.dma_addr);
1226         }
1227 }
1228
1229 void tw5864_prepare_frame_headers(struct tw5864_input *input)
1230 {
1231         struct tw5864_buf *vb = input->vb;
1232         u8 *dst;
1233         size_t dst_space;
1234         unsigned long flags;
1235
1236         if (!vb) {
1237                 spin_lock_irqsave(&input->slock, flags);
1238                 if (list_empty(&input->active)) {
1239                         spin_unlock_irqrestore(&input->slock, flags);
1240                         input->vb = NULL;
1241                         return;
1242                 }
1243                 vb = list_first_entry(&input->active, struct tw5864_buf, list);
1244                 list_del(&vb->list);
1245                 spin_unlock_irqrestore(&input->slock, flags);
1246         }
1247
1248         dst = vb2_plane_vaddr(&vb->vb.vb2_buf, 0);
1249         dst_space = vb2_plane_size(&vb->vb.vb2_buf, 0);
1250
1251         /*
1252          * Low-level bitstream writing functions don't have a fine way to say
1253          * correctly that supplied buffer is too small. So we just check there
1254          * and warn, and don't care at lower level.
1255          * Currently all headers take below 32 bytes.
1256          * The buffer is supposed to have plenty of free space at this point,
1257          * anyway.
1258          */
1259         if (WARN_ON_ONCE(dst_space < 128))
1260                 return;
1261
1262         /*
1263          * Generate H264 headers:
1264          * If this is first frame, put SPS and PPS
1265          */
1266         if (input->frame_gop_seqno == 0)
1267                 tw5864_h264_put_stream_header(&dst, &dst_space, input->qp,
1268                                               input->width, input->height);
1269
1270         /* Put slice header */
1271         tw5864_h264_put_slice_header(&dst, &dst_space, input->h264_idr_pic_id,
1272                                      input->frame_gop_seqno,
1273                                      &input->tail_nb_bits, &input->tail);
1274         input->vb = vb;
1275         input->buf_cur_ptr = dst;
1276         input->buf_cur_space_left = dst_space;
1277 }
1278
1279 /*
1280  * Returns heuristic motion detection metric value from known components of
1281  * hardware-provided Motion Vector Data.
1282  */
1283 static unsigned int tw5864_md_metric_from_mvd(u32 mvd)
1284 {
1285         /*
1286          * Format of motion vector data exposed by tw5864, according to
1287          * manufacturer:
1288          * mv_x 10 bits
1289          * mv_y 10 bits
1290          * non_zero_members 8 bits
1291          * mb_type 3 bits
1292          * reserved 1 bit
1293          *
1294          * non_zero_members: number of non-zero residuals in each macro block
1295          * after quantization
1296          *
1297          * unsigned int reserved = mvd >> 31;
1298          * unsigned int mb_type = (mvd >> 28) & 0x7;
1299          * unsigned int non_zero_members = (mvd >> 20) & 0xff;
1300          */
1301         unsigned int mv_y = (mvd >> 10) & 0x3ff;
1302         unsigned int mv_x = mvd & 0x3ff;
1303
1304         /* heuristic: */
1305         mv_x &= 0x0f;
1306         mv_y &= 0x0f;
1307
1308         return mv_y + mv_x;
1309 }
1310
1311 static int tw5864_is_motion_triggered(struct tw5864_h264_frame *frame)
1312 {
1313         struct tw5864_input *input = frame->input;
1314         u32 *mv = (u32 *)frame->mv.addr;
1315         int i;
1316         int detected = 0;
1317
1318         for (i = 0; i < MD_CELLS; i++) {
1319                 const u16 thresh = input->md_threshold_grid_values[i];
1320                 const unsigned int metric = tw5864_md_metric_from_mvd(mv[i]);
1321
1322                 if (metric > thresh)
1323                         detected = 1;
1324
1325                 if (detected)
1326                         break;
1327         }
1328         return detected;
1329 }
1330
1331 static void tw5864_handle_frame_task(unsigned long data)
1332 {
1333         struct tw5864_dev *dev = (struct tw5864_dev *)data;
1334         unsigned long flags;
1335         int batch_size = H264_BUF_CNT;
1336
1337         spin_lock_irqsave(&dev->slock, flags);
1338         while (dev->h264_buf_r_index != dev->h264_buf_w_index && batch_size--) {
1339                 struct tw5864_h264_frame *frame =
1340                         &dev->h264_buf[dev->h264_buf_r_index];
1341
1342                 spin_unlock_irqrestore(&dev->slock, flags);
1343                 dma_sync_single_for_cpu(&dev->pci->dev, frame->vlc.dma_addr,
1344                                         H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1345                 dma_sync_single_for_cpu(&dev->pci->dev, frame->mv.dma_addr,
1346                                         H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1347                 tw5864_handle_frame(frame);
1348                 dma_sync_single_for_device(&dev->pci->dev, frame->vlc.dma_addr,
1349                                            H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1350                 dma_sync_single_for_device(&dev->pci->dev, frame->mv.dma_addr,
1351                                            H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1352                 spin_lock_irqsave(&dev->slock, flags);
1353
1354                 dev->h264_buf_r_index++;
1355                 dev->h264_buf_r_index %= H264_BUF_CNT;
1356         }
1357         spin_unlock_irqrestore(&dev->slock, flags);
1358 }
1359
1360 #ifdef DEBUG
1361 static u32 tw5864_vlc_checksum(u32 *data, int len)
1362 {
1363         u32 val, count_len = len;
1364
1365         val = *data++;
1366         while (((count_len >> 2) - 1) > 0) {
1367                 val ^= *data++;
1368                 count_len -= 4;
1369         }
1370         val ^= htonl((len >> 2));
1371         return val;
1372 }
1373 #endif
1374
1375 static void tw5864_handle_frame(struct tw5864_h264_frame *frame)
1376 {
1377 #define SKIP_VLCBUF_BYTES 3
1378         struct tw5864_input *input = frame->input;
1379         struct tw5864_dev *dev = input->root;
1380         struct tw5864_buf *vb;
1381         struct vb2_v4l2_buffer *v4l2_buf;
1382         int frame_len = frame->vlc_len - SKIP_VLCBUF_BYTES;
1383         u8 *dst = input->buf_cur_ptr;
1384         u8 tail_mask, vlc_mask = 0;
1385         int i;
1386         u8 vlc_first_byte = ((u8 *)(frame->vlc.addr + SKIP_VLCBUF_BYTES))[0];
1387         unsigned long flags;
1388         int zero_run;
1389         u8 *src;
1390         u8 *src_end;
1391
1392 #ifdef DEBUG
1393         if (frame->checksum !=
1394             tw5864_vlc_checksum((u32 *)frame->vlc.addr, frame_len))
1395                 dev_err(&dev->pci->dev,
1396                         "Checksum of encoded frame doesn't match!\n");
1397 #endif
1398
1399         spin_lock_irqsave(&input->slock, flags);
1400         vb = input->vb;
1401         input->vb = NULL;
1402         spin_unlock_irqrestore(&input->slock, flags);
1403
1404         if (!vb) { /* Gone because of disabling */
1405                 dev_dbg(&dev->pci->dev, "vb is empty, dropping frame\n");
1406                 return;
1407         }
1408
1409         v4l2_buf = to_vb2_v4l2_buffer(&vb->vb.vb2_buf);
1410
1411         /*
1412          * Check for space.
1413          * Mind the overhead of startcode emulation prevention.
1414          */
1415         if (input->buf_cur_space_left < frame_len * 5 / 4) {
1416                 dev_err_once(&dev->pci->dev,
1417                              "Left space in vb2 buffer, %d bytes, is less than considered safely enough to put frame of length %d. Dropping this frame.\n",
1418                              input->buf_cur_space_left, frame_len);
1419                 return;
1420         }
1421
1422         for (i = 0; i < 8 - input->tail_nb_bits; i++)
1423                 vlc_mask |= 1 << i;
1424         tail_mask = (~vlc_mask) & 0xff;
1425
1426         dst[0] = (input->tail & tail_mask) | (vlc_first_byte & vlc_mask);
1427         frame_len--;
1428         dst++;
1429
1430         /* H.264 startcode emulation prevention */
1431         src = frame->vlc.addr + SKIP_VLCBUF_BYTES + 1;
1432         src_end = src + frame_len;
1433         zero_run = 0;
1434         for (; src < src_end; src++) {
1435                 if (zero_run < 2) {
1436                         if (*src == 0)
1437                                 ++zero_run;
1438                         else
1439                                 zero_run = 0;
1440                 } else {
1441                         if ((*src & ~0x03) == 0)
1442                                 *dst++ = 0x03;
1443                         zero_run = *src == 0;
1444                 }
1445                 *dst++ = *src;
1446         }
1447
1448         vb2_set_plane_payload(&vb->vb.vb2_buf, 0,
1449                               dst - (u8 *)vb2_plane_vaddr(&vb->vb.vb2_buf, 0));
1450
1451         vb->vb.vb2_buf.timestamp = frame->timestamp;
1452         v4l2_buf->field = V4L2_FIELD_INTERLACED;
1453         v4l2_buf->sequence = frame->seqno;
1454
1455         /* Check for motion flags */
1456         if (frame->gop_seqno /* P-frame */ &&
1457             tw5864_is_motion_triggered(frame)) {
1458                 struct v4l2_event ev = {
1459                         .type = V4L2_EVENT_MOTION_DET,
1460                         .u.motion_det = {
1461                                 .flags = V4L2_EVENT_MD_FL_HAVE_FRAME_SEQ,
1462                                 .frame_sequence = v4l2_buf->sequence,
1463                         },
1464                 };
1465
1466                 v4l2_event_queue(&input->vdev, &ev);
1467         }
1468
1469         vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
1470 }
1471
1472 static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std std)
1473 {
1474         switch (std) {
1475         case STD_NTSC:    return V4L2_STD_NTSC_M;
1476         case STD_PAL:     return V4L2_STD_PAL_B;
1477         case STD_SECAM:   return V4L2_STD_SECAM_B;
1478         case STD_NTSC443: return V4L2_STD_NTSC_443;
1479         case STD_PAL_M:   return V4L2_STD_PAL_M;
1480         case STD_PAL_CN:  return V4L2_STD_PAL_Nc;
1481         case STD_PAL_60:  return V4L2_STD_PAL_60;
1482         case STD_INVALID: return V4L2_STD_UNKNOWN;
1483         }
1484         return 0;
1485 }
1486
1487 static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std)
1488 {
1489         if (v4l2_std & V4L2_STD_NTSC_M)
1490                 return STD_NTSC;
1491         if (v4l2_std & V4L2_STD_PAL_B)
1492                 return STD_PAL;
1493         if (v4l2_std & V4L2_STD_SECAM_B)
1494                 return STD_SECAM;
1495         if (v4l2_std & V4L2_STD_NTSC_443)
1496                 return STD_NTSC443;
1497         if (v4l2_std & V4L2_STD_PAL_M)
1498                 return STD_PAL_M;
1499         if (v4l2_std & V4L2_STD_PAL_Nc)
1500                 return STD_PAL_CN;
1501         if (v4l2_std & V4L2_STD_PAL_60)
1502                 return STD_PAL_60;
1503
1504         return STD_INVALID;
1505 }
1506
1507 static void tw5864_encoder_tables_upload(struct tw5864_dev *dev)
1508 {
1509         int i;
1510
1511         tw_writel(TW5864_VLC_RD, 0x1);
1512         for (i = 0; i < VLC_LOOKUP_TABLE_LEN; i++) {
1513                 tw_writel((TW5864_VLC_STREAM_MEM_START + i * 4),
1514                           encoder_vlc_lookup_table[i]);
1515         }
1516         tw_writel(TW5864_VLC_RD, 0x0);
1517
1518         for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1519                 tw_writel((TW5864_QUAN_TAB + i * 4),
1520                           forward_quantization_table[i]);
1521         }
1522
1523         for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1524                 tw_writel((TW5864_QUAN_TAB + i * 4),
1525                           inverse_quantization_table[i]);
1526         }
1527 }