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

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