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         sprintf(cap->bus_info, "PCI:%s", pci_name(input->root->pci));
 608         return 0;
 609 }
 610
 611 static int tw5864_querystd(struct file *file, void *priv, v4l2_std_id *std)
 612 {
 613         struct tw5864_input *input = video_drvdata(file);
 614         enum tw5864_vid_std tw_std;
 615         int ret;
 616
 617         ret = tw5864_input_std_get(input, &tw_std);
 618         if (ret)
 619                 return ret;
 620         *std = tw5864_get_v4l2_std(tw_std);
 621
 622         return 0;
 623 }
 624
 625 static int tw5864_g_std(struct file *file, void *priv, v4l2_std_id *std)
 626 {
 627         struct tw5864_input *input = video_drvdata(file);
 628
 629         *std = input->v4l2_std;
 630         return 0;
 631 }
 632
 633 static int tw5864_s_std(struct file *file, void *priv, v4l2_std_id std)
 634 {
 635         struct tw5864_input *input = video_drvdata(file);
 636         struct tw5864_dev *dev = input->root;
 637
 638         input->v4l2_std = std;
 639         input->std = tw5864_from_v4l2_std(std);
 640         tw_indir_writeb(TW5864_INDIR_VIN_E(input->nr), input->std);
 641         return 0;
 642 }
 643
 644 static int tw5864_enum_fmt_vid_cap(struct file *file, void *priv,
 645                                    struct v4l2_fmtdesc *f)
 646 {
 647         if (f->index)
 648                 return -EINVAL;
 649
 650         f->pixelformat = V4L2_PIX_FMT_H264;
 651
 652         return 0;
 653 }
 654
 655 static int tw5864_subscribe_event(struct v4l2_fh *fh,
 656                                   const struct v4l2_event_subscription *sub)
 657 {
 658         switch (sub->type) {
 659         case V4L2_EVENT_MOTION_DET:
 660                 /*
 661                  * Allow for up to 30 events (1 second for NTSC) to be stored.
 662                  */
 663                 return v4l2_event_subscribe(fh, sub, 30, NULL);
 664         default:
 665                 return v4l2_ctrl_subscribe_event(fh, sub);
 666         }
 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 = 704;
 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_setup(&dev->tasklet, tw5864_handle_frame_task);
1067
1068         for (i = 0; i < TW5864_INPUTS; i++) {
1069                 dev->inputs[i].root = dev;
1070                 dev->inputs[i].nr = i;
1071                 ret = tw5864_video_input_init(&dev->inputs[i], video_nr[i]);
1072                 if (ret)
1073                         goto fini_video_inputs;
1074                 last_input_nr_registered = i;
1075         }
1076
1077         return 0;
1078
1079 fini_video_inputs:
1080         for (i = last_input_nr_registered; i >= 0; i--)
1081                 tw5864_video_input_fini(&dev->inputs[i]);
1082
1083         tasklet_kill(&dev->tasklet);
1084
1085 free_dma:
1086         for (i = last_dma_allocated; i >= 0; i--) {
1087                 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1088                                   dev->h264_buf[i].vlc.addr,
1089                                   dev->h264_buf[i].vlc.dma_addr);
1090                 dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1091                                   dev->h264_buf[i].mv.addr,
1092                                   dev->h264_buf[i].mv.dma_addr);
1093         }
1094
1095         return ret;
1096 }
1097
1098 static int tw5864_video_input_init(struct tw5864_input *input, int video_nr)
1099 {
1100         struct tw5864_dev *dev = input->root;
1101         int ret;
1102         struct v4l2_ctrl_handler *hdl = &input->hdl;
1103
1104         mutex_init(&input->lock);
1105         spin_lock_init(&input->slock);
1106
1107         /* setup video buffers queue */
1108         INIT_LIST_HEAD(&input->active);
1109         input->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1110         input->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1111         input->vidq.io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
1112         input->vidq.ops = &tw5864_video_qops;
1113         input->vidq.mem_ops = &vb2_dma_contig_memops;
1114         input->vidq.drv_priv = input;
1115         input->vidq.gfp_flags = 0;
1116         input->vidq.buf_struct_size = sizeof(struct tw5864_buf);
1117         input->vidq.lock = &input->lock;
1118         input->vidq.min_buffers_needed = 2;
1119         input->vidq.dev = &input->root->pci->dev;
1120         ret = vb2_queue_init(&input->vidq);
1121         if (ret)
1122                 goto free_mutex;
1123
1124         input->vdev = tw5864_video_template;
1125         input->vdev.v4l2_dev = &input->root->v4l2_dev;
1126         input->vdev.lock = &input->lock;
1127         input->vdev.queue = &input->vidq;
1128         video_set_drvdata(&input->vdev, input);
1129
1130         /* Initialize the device control structures */
1131         v4l2_ctrl_handler_init(hdl, 6);
1132         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1133                           V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1134         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1135                           V4L2_CID_CONTRAST, 0, 255, 1, 100);
1136         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1137                           V4L2_CID_SATURATION, 0, 255, 1, 128);
1138         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_HUE, -128, 127, 1, 0);
1139         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1140                           1, MAX_GOP_SIZE, 1, GOP_SIZE);
1141         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1142                           V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 28, 51, 1, QP_VALUE);
1143         v4l2_ctrl_new_std_menu(hdl, &tw5864_ctrl_ops,
1144                                V4L2_CID_DETECT_MD_MODE,
1145                                V4L2_DETECT_MD_MODE_THRESHOLD_GRID, 0,
1146                                V4L2_DETECT_MD_MODE_DISABLED);
1147         v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1148                           V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD,
1149                           tw5864_md_thresholds.min, tw5864_md_thresholds.max,
1150                           tw5864_md_thresholds.step, tw5864_md_thresholds.def);
1151         input->md_threshold_grid_ctrl =
1152                 v4l2_ctrl_new_custom(hdl, &tw5864_md_thresholds, NULL);
1153         if (hdl->error) {
1154                 ret = hdl->error;
1155                 goto free_v4l2_hdl;
1156         }
1157         input->vdev.ctrl_handler = hdl;
1158         v4l2_ctrl_handler_setup(hdl);
1159
1160         input->qp = QP_VALUE;
1161         input->gop = GOP_SIZE;
1162         input->frame_interval = 1;
1163
1164         ret = video_register_device(&input->vdev, VFL_TYPE_VIDEO, video_nr);
1165         if (ret)
1166                 goto free_v4l2_hdl;
1167
1168         dev_info(&input->root->pci->dev, "Registered video device %s\n",
1169                  video_device_node_name(&input->vdev));
1170
1171         /*
1172          * Set default video standard. Doesn't matter which, the detected value
1173          * will be found out by VIDIOC_QUERYSTD handler.
1174          */
1175         input->v4l2_std = V4L2_STD_NTSC_M;
1176         input->std = STD_NTSC;
1177
1178         tw_indir_writeb(TW5864_INDIR_VIN_E(video_nr), 0x07);
1179         /* to initiate auto format recognition */
1180         tw_indir_writeb(TW5864_INDIR_VIN_F(video_nr), 0xff);
1181
1182         return 0;
1183
1184 free_v4l2_hdl:
1185         v4l2_ctrl_handler_free(hdl);
1186 free_mutex:
1187         mutex_destroy(&input->lock);
1188
1189         return ret;
1190 }
1191
1192 static void tw5864_video_input_fini(struct tw5864_input *dev)
1193 {
1194         vb2_video_unregister_device(&dev->vdev);
1195         v4l2_ctrl_handler_free(&dev->hdl);
1196 }
1197
1198 void tw5864_video_fini(struct tw5864_dev *dev)
1199 {
1200         int i;
1201
1202         tasklet_kill(&dev->tasklet);
1203
1204         for (i = 0; i < TW5864_INPUTS; i++)
1205                 tw5864_video_input_fini(&dev->inputs[i]);
1206
1207         for (i = 0; i < H264_BUF_CNT; i++) {
1208                 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1209                                   dev->h264_buf[i].vlc.addr,
1210                                   dev->h264_buf[i].vlc.dma_addr);
1211                 dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1212                                   dev->h264_buf[i].mv.addr,
1213                                   dev->h264_buf[i].mv.dma_addr);
1214         }
1215 }
1216
1217 void tw5864_prepare_frame_headers(struct tw5864_input *input)
1218 {
1219         struct tw5864_buf *vb = input->vb;
1220         u8 *dst;
1221         size_t dst_space;
1222         unsigned long flags;
1223
1224         if (!vb) {
1225                 spin_lock_irqsave(&input->slock, flags);
1226                 if (list_empty(&input->active)) {
1227                         spin_unlock_irqrestore(&input->slock, flags);
1228                         input->vb = NULL;
1229                         return;
1230                 }
1231                 vb = list_first_entry(&input->active, struct tw5864_buf, list);
1232                 list_del(&vb->list);
1233                 spin_unlock_irqrestore(&input->slock, flags);
1234         }
1235
1236         dst = vb2_plane_vaddr(&vb->vb.vb2_buf, 0);
1237         dst_space = vb2_plane_size(&vb->vb.vb2_buf, 0);
1238
1239         /*
1240          * Low-level bitstream writing functions don't have a fine way to say
1241          * correctly that supplied buffer is too small. So we just check there
1242          * and warn, and don't care at lower level.
1243          * Currently all headers take below 32 bytes.
1244          * The buffer is supposed to have plenty of free space at this point,
1245          * anyway.
1246          */
1247         if (WARN_ON_ONCE(dst_space < 128))
1248                 return;
1249
1250         /*
1251          * Generate H264 headers:
1252          * If this is first frame, put SPS and PPS
1253          */
1254         if (input->frame_gop_seqno == 0)
1255                 tw5864_h264_put_stream_header(&dst, &dst_space, input->qp,
1256                                               input->width, input->height);
1257
1258         /* Put slice header */
1259         tw5864_h264_put_slice_header(&dst, &dst_space, input->h264_idr_pic_id,
1260                                      input->frame_gop_seqno,
1261                                      &input->tail_nb_bits, &input->tail);
1262         input->vb = vb;
1263         input->buf_cur_ptr = dst;
1264         input->buf_cur_space_left = dst_space;
1265 }
1266
1267 /*
1268  * Returns heuristic motion detection metric value from known components of
1269  * hardware-provided Motion Vector Data.
1270  */
1271 static unsigned int tw5864_md_metric_from_mvd(u32 mvd)
1272 {
1273         /*
1274          * Format of motion vector data exposed by tw5864, according to
1275          * manufacturer:
1276          * mv_x 10 bits
1277          * mv_y 10 bits
1278          * non_zero_members 8 bits
1279          * mb_type 3 bits
1280          * reserved 1 bit
1281          *
1282          * non_zero_members: number of non-zero residuals in each macro block
1283          * after quantization
1284          *
1285          * unsigned int reserved = mvd >> 31;
1286          * unsigned int mb_type = (mvd >> 28) & 0x7;
1287          * unsigned int non_zero_members = (mvd >> 20) & 0xff;
1288          */
1289         unsigned int mv_y = (mvd >> 10) & 0x3ff;
1290         unsigned int mv_x = mvd & 0x3ff;
1291
1292         /* heuristic: */
1293         mv_x &= 0x0f;
1294         mv_y &= 0x0f;
1295
1296         return mv_y + mv_x;
1297 }
1298
1299 static int tw5864_is_motion_triggered(struct tw5864_h264_frame *frame)
1300 {
1301         struct tw5864_input *input = frame->input;
1302         u32 *mv = (u32 *)frame->mv.addr;
1303         int i;
1304         int detected = 0;
1305
1306         for (i = 0; i < MD_CELLS; i++) {
1307                 const u16 thresh = input->md_threshold_grid_values[i];
1308                 const unsigned int metric = tw5864_md_metric_from_mvd(mv[i]);
1309
1310                 if (metric > thresh)
1311                         detected = 1;
1312
1313                 if (detected)
1314                         break;
1315         }
1316         return detected;
1317 }
1318
1319 static void tw5864_handle_frame_task(struct tasklet_struct *t)
1320 {
1321         struct tw5864_dev *dev = from_tasklet(dev, t, tasklet);
1322         unsigned long flags;
1323         int batch_size = H264_BUF_CNT;
1324
1325         spin_lock_irqsave(&dev->slock, flags);
1326         while (dev->h264_buf_r_index != dev->h264_buf_w_index && batch_size--) {
1327                 struct tw5864_h264_frame *frame =
1328                         &dev->h264_buf[dev->h264_buf_r_index];
1329
1330                 spin_unlock_irqrestore(&dev->slock, flags);
1331                 dma_sync_single_for_cpu(&dev->pci->dev, frame->vlc.dma_addr,
1332                                         H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1333                 dma_sync_single_for_cpu(&dev->pci->dev, frame->mv.dma_addr,
1334                                         H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1335                 tw5864_handle_frame(frame);
1336                 dma_sync_single_for_device(&dev->pci->dev, frame->vlc.dma_addr,
1337                                            H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1338                 dma_sync_single_for_device(&dev->pci->dev, frame->mv.dma_addr,
1339                                            H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1340                 spin_lock_irqsave(&dev->slock, flags);
1341
1342                 dev->h264_buf_r_index++;
1343                 dev->h264_buf_r_index %= H264_BUF_CNT;
1344         }
1345         spin_unlock_irqrestore(&dev->slock, flags);
1346 }
1347
1348 #ifdef DEBUG
1349 static u32 tw5864_vlc_checksum(u32 *data, int len)
1350 {
1351         u32 val, count_len = len;
1352
1353         val = *data++;
1354         while (((count_len >> 2) - 1) > 0) {
1355                 val ^= *data++;
1356                 count_len -= 4;
1357         }
1358         val ^= htonl((len >> 2));
1359         return val;
1360 }
1361 #endif
1362
1363 static void tw5864_handle_frame(struct tw5864_h264_frame *frame)
1364 {
1365 #define SKIP_VLCBUF_BYTES 3
1366         struct tw5864_input *input = frame->input;
1367         struct tw5864_dev *dev = input->root;
1368         struct tw5864_buf *vb;
1369         struct vb2_v4l2_buffer *v4l2_buf;
1370         int frame_len = frame->vlc_len - SKIP_VLCBUF_BYTES;
1371         u8 *dst = input->buf_cur_ptr;
1372         u8 tail_mask, vlc_mask = 0;
1373         int i;
1374         u8 vlc_first_byte = ((u8 *)(frame->vlc.addr + SKIP_VLCBUF_BYTES))[0];
1375         unsigned long flags;
1376         int zero_run;
1377         u8 *src;
1378         u8 *src_end;
1379
1380 #ifdef DEBUG
1381         if (frame->checksum !=
1382             tw5864_vlc_checksum((u32 *)frame->vlc.addr, frame_len))
1383                 dev_err(&dev->pci->dev,
1384                         "Checksum of encoded frame doesn't match!\n");
1385 #endif
1386
1387         spin_lock_irqsave(&input->slock, flags);
1388         vb = input->vb;
1389         input->vb = NULL;
1390         spin_unlock_irqrestore(&input->slock, flags);
1391
1392         if (!vb) { /* Gone because of disabling */
1393                 dev_dbg(&dev->pci->dev, "vb is empty, dropping frame\n");
1394                 return;
1395         }
1396
1397         v4l2_buf = to_vb2_v4l2_buffer(&vb->vb.vb2_buf);
1398
1399         /*
1400          * Check for space.
1401          * Mind the overhead of startcode emulation prevention.
1402          */
1403         if (input->buf_cur_space_left < frame_len * 5 / 4) {
1404                 dev_err_once(&dev->pci->dev,
1405                              "Left space in vb2 buffer, %d bytes, is less than considered safely enough to put frame of length %d. Dropping this frame.\n",
1406                              input->buf_cur_space_left, frame_len);
1407                 return;
1408         }
1409
1410         for (i = 0; i < 8 - input->tail_nb_bits; i++)
1411                 vlc_mask |= 1 << i;
1412         tail_mask = (~vlc_mask) & 0xff;
1413
1414         dst[0] = (input->tail & tail_mask) | (vlc_first_byte & vlc_mask);
1415         frame_len--;
1416         dst++;
1417
1418         /* H.264 startcode emulation prevention */
1419         src = frame->vlc.addr + SKIP_VLCBUF_BYTES + 1;
1420         src_end = src + frame_len;
1421         zero_run = 0;
1422         for (; src < src_end; src++) {
1423                 if (zero_run < 2) {
1424                         if (*src == 0)
1425                                 ++zero_run;
1426                         else
1427                                 zero_run = 0;
1428                 } else {
1429                         if ((*src & ~0x03) == 0)
1430                                 *dst++ = 0x03;
1431                         zero_run = *src == 0;
1432                 }
1433                 *dst++ = *src;
1434         }
1435
1436         vb2_set_plane_payload(&vb->vb.vb2_buf, 0,
1437                               dst - (u8 *)vb2_plane_vaddr(&vb->vb.vb2_buf, 0));
1438
1439         vb->vb.vb2_buf.timestamp = frame->timestamp;
1440         v4l2_buf->field = V4L2_FIELD_INTERLACED;
1441         v4l2_buf->sequence = frame->seqno;
1442
1443         /* Check for motion flags */
1444         if (frame->gop_seqno /* P-frame */ &&
1445             tw5864_is_motion_triggered(frame)) {
1446                 struct v4l2_event ev = {
1447                         .type = V4L2_EVENT_MOTION_DET,
1448                         .u.motion_det = {
1449                                 .flags = V4L2_EVENT_MD_FL_HAVE_FRAME_SEQ,
1450                                 .frame_sequence = v4l2_buf->sequence,
1451                         },
1452                 };
1453
1454                 v4l2_event_queue(&input->vdev, &ev);
1455         }
1456
1457         vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
1458 }
1459
1460 static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std std)
1461 {
1462         switch (std) {
1463         case STD_NTSC:    return V4L2_STD_NTSC_M;
1464         case STD_PAL:     return V4L2_STD_PAL_B;
1465         case STD_SECAM:   return V4L2_STD_SECAM_B;
1466         case STD_NTSC443: return V4L2_STD_NTSC_443;
1467         case STD_PAL_M:   return V4L2_STD_PAL_M;
1468         case STD_PAL_CN:  return V4L2_STD_PAL_Nc;
1469         case STD_PAL_60:  return V4L2_STD_PAL_60;
1470         case STD_INVALID: return V4L2_STD_UNKNOWN;
1471         }
1472         return 0;
1473 }
1474
1475 static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std)
1476 {
1477         if (v4l2_std & V4L2_STD_NTSC_M)
1478                 return STD_NTSC;
1479         if (v4l2_std & V4L2_STD_PAL_B)
1480                 return STD_PAL;
1481         if (v4l2_std & V4L2_STD_SECAM_B)
1482                 return STD_SECAM;
1483         if (v4l2_std & V4L2_STD_NTSC_443)
1484                 return STD_NTSC443;
1485         if (v4l2_std & V4L2_STD_PAL_M)
1486                 return STD_PAL_M;
1487         if (v4l2_std & V4L2_STD_PAL_Nc)
1488                 return STD_PAL_CN;
1489         if (v4l2_std & V4L2_STD_PAL_60)
1490                 return STD_PAL_60;
1491
1492         return STD_INVALID;
1493 }
1494
1495 static void tw5864_encoder_tables_upload(struct tw5864_dev *dev)
1496 {
1497         int i;
1498
1499         tw_writel(TW5864_VLC_RD, 0x1);
1500         for (i = 0; i < VLC_LOOKUP_TABLE_LEN; i++) {
1501                 tw_writel((TW5864_VLC_STREAM_MEM_START + i * 4),
1502                           encoder_vlc_lookup_table[i]);
1503         }
1504         tw_writel(TW5864_VLC_RD, 0x0);
1505
1506         for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1507                 tw_writel((TW5864_QUAN_TAB + i * 4),
1508                           forward_quantization_table[i]);
1509         }
1510
1511         for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1512                 tw_writel((TW5864_QUAN_TAB + i * 4),
1513                           inverse_quantization_table[i]);
1514         }
1515 }