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