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_VIDEO, 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 }
1513