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