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