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