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

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