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