1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * TW5864 driver - video encoding functions
4 *
5 * Copyright (C) 2016 Bluecherry, LLC <[email protected]>
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 };
475
tw5864_s_ctrl(struct v4l2_ctrl * ctrl)476 static int tw5864_s_ctrl(struct v4l2_ctrl *ctrl)
477 {
478 struct tw5864_input *input =
479 container_of(ctrl->handler, struct tw5864_input, hdl);
480 struct tw5864_dev *dev = input->root;
481 unsigned long flags;
482
483 switch (ctrl->id) {
484 case V4L2_CID_BRIGHTNESS:
485 tw_indir_writeb(TW5864_INDIR_VIN_A_BRIGHT(input->nr),
486 (u8)ctrl->val);
487 break;
488 case V4L2_CID_HUE:
489 tw_indir_writeb(TW5864_INDIR_VIN_7_HUE(input->nr),
490 (u8)ctrl->val);
491 break;
492 case V4L2_CID_CONTRAST:
493 tw_indir_writeb(TW5864_INDIR_VIN_9_CNTRST(input->nr),
494 (u8)ctrl->val);
495 break;
496 case V4L2_CID_SATURATION:
497 tw_indir_writeb(TW5864_INDIR_VIN_B_SAT_U(input->nr),
498 (u8)ctrl->val);
499 tw_indir_writeb(TW5864_INDIR_VIN_C_SAT_V(input->nr),
500 (u8)ctrl->val);
501 break;
502 case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
503 input->gop = ctrl->val;
504 return 0;
505 case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
506 spin_lock_irqsave(&input->slock, flags);
507 input->qp = ctrl->val;
508 input->reg_dsp_qp = input->qp;
509 input->reg_dsp_ref_mvp_lambda = lambda_lookup_table[input->qp];
510 input->reg_dsp_i4x4_weight = intra4x4_lambda3[input->qp];
511 spin_unlock_irqrestore(&input->slock, flags);
512 return 0;
513 case V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD:
514 memset(input->md_threshold_grid_values, ctrl->val,
515 sizeof(input->md_threshold_grid_values));
516 return 0;
517 case V4L2_CID_DETECT_MD_MODE:
518 return 0;
519 case V4L2_CID_DETECT_MD_THRESHOLD_GRID:
520 /* input->md_threshold_grid_ctrl->p_new.p_u16 contains data */
521 memcpy(input->md_threshold_grid_values,
522 input->md_threshold_grid_ctrl->p_new.p_u16,
523 sizeof(input->md_threshold_grid_values));
524 return 0;
525 }
526 return 0;
527 }
528
tw5864_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)529 static int tw5864_fmt_vid_cap(struct file *file, void *priv,
530 struct v4l2_format *f)
531 {
532 struct tw5864_input *input = video_drvdata(file);
533
534 f->fmt.pix.width = 704;
535 switch (input->std) {
536 default:
537 WARN_ON_ONCE(1);
538 return -EINVAL;
539 case STD_NTSC:
540 f->fmt.pix.height = 480;
541 break;
542 case STD_PAL:
543 case STD_SECAM:
544 f->fmt.pix.height = 576;
545 break;
546 }
547 f->fmt.pix.field = V4L2_FIELD_INTERLACED;
548 f->fmt.pix.pixelformat = V4L2_PIX_FMT_H264;
549 f->fmt.pix.sizeimage = H264_VLC_BUF_SIZE;
550 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
551 return 0;
552 }
553
tw5864_enum_input(struct file * file,void * priv,struct v4l2_input * i)554 static int tw5864_enum_input(struct file *file, void *priv,
555 struct v4l2_input *i)
556 {
557 struct tw5864_input *input = video_drvdata(file);
558 struct tw5864_dev *dev = input->root;
559
560 u8 indir_0x000 = tw_indir_readb(TW5864_INDIR_VIN_0(input->nr));
561 u8 indir_0x00d = tw_indir_readb(TW5864_INDIR_VIN_D(input->nr));
562 u8 v1 = indir_0x000;
563 u8 v2 = indir_0x00d;
564
565 if (i->index)
566 return -EINVAL;
567
568 i->type = V4L2_INPUT_TYPE_CAMERA;
569 snprintf(i->name, sizeof(i->name), "Encoder %d", input->nr);
570 i->std = TW5864_NORMS;
571 if (v1 & (1 << 7))
572 i->status |= V4L2_IN_ST_NO_SYNC;
573 if (!(v1 & (1 << 6)))
574 i->status |= V4L2_IN_ST_NO_H_LOCK;
575 if (v1 & (1 << 2))
576 i->status |= V4L2_IN_ST_NO_SIGNAL;
577 if (v1 & (1 << 1))
578 i->status |= V4L2_IN_ST_NO_COLOR;
579 if (v2 & (1 << 2))
580 i->status |= V4L2_IN_ST_MACROVISION;
581
582 return 0;
583 }
584
tw5864_g_input(struct file * file,void * priv,unsigned int * i)585 static int tw5864_g_input(struct file *file, void *priv, unsigned int *i)
586 {
587 *i = 0;
588 return 0;
589 }
590
tw5864_s_input(struct file * file,void * priv,unsigned int i)591 static int tw5864_s_input(struct file *file, void *priv, unsigned int i)
592 {
593 if (i)
594 return -EINVAL;
595 return 0;
596 }
597
tw5864_querycap(struct file * file,void * priv,struct v4l2_capability * cap)598 static int tw5864_querycap(struct file *file, void *priv,
599 struct v4l2_capability *cap)
600 {
601 struct tw5864_input *input = video_drvdata(file);
602
603 strscpy(cap->driver, "tw5864", sizeof(cap->driver));
604 snprintf(cap->card, sizeof(cap->card), "TW5864 Encoder %d",
605 input->nr);
606 return 0;
607 }
608
tw5864_querystd(struct file * file,void * priv,v4l2_std_id * std)609 static int tw5864_querystd(struct file *file, void *priv, v4l2_std_id *std)
610 {
611 struct tw5864_input *input = video_drvdata(file);
612 enum tw5864_vid_std tw_std;
613 int ret;
614
615 ret = tw5864_input_std_get(input, &tw_std);
616 if (ret)
617 return ret;
618 *std = tw5864_get_v4l2_std(tw_std);
619
620 return 0;
621 }
622
tw5864_g_std(struct file * file,void * priv,v4l2_std_id * std)623 static int tw5864_g_std(struct file *file, void *priv, v4l2_std_id *std)
624 {
625 struct tw5864_input *input = video_drvdata(file);
626
627 *std = input->v4l2_std;
628 return 0;
629 }
630
tw5864_s_std(struct file * file,void * priv,v4l2_std_id std)631 static int tw5864_s_std(struct file *file, void *priv, v4l2_std_id std)
632 {
633 struct tw5864_input *input = video_drvdata(file);
634 struct tw5864_dev *dev = input->root;
635
636 input->v4l2_std = std;
637 input->std = tw5864_from_v4l2_std(std);
638 tw_indir_writeb(TW5864_INDIR_VIN_E(input->nr), input->std);
639 return 0;
640 }
641
tw5864_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)642 static int tw5864_enum_fmt_vid_cap(struct file *file, void *priv,
643 struct v4l2_fmtdesc *f)
644 {
645 if (f->index)
646 return -EINVAL;
647
648 f->pixelformat = V4L2_PIX_FMT_H264;
649
650 return 0;
651 }
652
tw5864_subscribe_event(struct v4l2_fh * fh,const struct v4l2_event_subscription * sub)653 static int tw5864_subscribe_event(struct v4l2_fh *fh,
654 const struct v4l2_event_subscription *sub)
655 {
656 switch (sub->type) {
657 case V4L2_EVENT_MOTION_DET:
658 /*
659 * Allow for up to 30 events (1 second for NTSC) to be stored.
660 */
661 return v4l2_event_subscribe(fh, sub, 30, NULL);
662 default:
663 return v4l2_ctrl_subscribe_event(fh, sub);
664 }
665 }
666
tw5864_frame_interval_set(struct tw5864_input * input)667 static void tw5864_frame_interval_set(struct tw5864_input *input)
668 {
669 /*
670 * This register value seems to follow such approach: In each second
671 * interval, when processing Nth frame, it checks Nth bit of register
672 * value and, if the bit is 1, it processes the frame, otherwise the
673 * frame is discarded.
674 * So unary representation would work, but more or less equal gaps
675 * between the frames should be preserved.
676 *
677 * For 1 FPS - 0x00000001
678 * 00000000 00000000 00000000 00000001
679 *
680 * For max FPS - set all 25/30 lower bits:
681 * 00111111 11111111 11111111 11111111 (NTSC)
682 * 00000001 11111111 11111111 11111111 (PAL)
683 *
684 * For half of max FPS - use such pattern:
685 * 00010101 01010101 01010101 01010101 (NTSC)
686 * 00000001 01010101 01010101 01010101 (PAL)
687 *
688 * Et cetera.
689 *
690 * The value supplied to hardware is capped by mask of 25/30 lower bits.
691 */
692 struct tw5864_dev *dev = input->root;
693 u32 unary_framerate = 0;
694 int shift = 0;
695 int std_max_fps = input->std == STD_NTSC ? 30 : 25;
696
697 for (shift = 0; shift < std_max_fps; shift += input->frame_interval)
698 unary_framerate |= 0x00000001 << shift;
699
700 tw_writel(TW5864_H264EN_RATE_CNTL_LO_WORD(input->nr, 0),
701 unary_framerate >> 16);
702 tw_writel(TW5864_H264EN_RATE_CNTL_HI_WORD(input->nr, 0),
703 unary_framerate & 0xffff);
704 }
705
tw5864_frameinterval_get(struct tw5864_input * input,struct v4l2_fract * frameinterval)706 static int tw5864_frameinterval_get(struct tw5864_input *input,
707 struct v4l2_fract *frameinterval)
708 {
709 struct tw5864_dev *dev = input->root;
710
711 switch (input->std) {
712 case STD_NTSC:
713 frameinterval->numerator = 1001;
714 frameinterval->denominator = 30000;
715 break;
716 case STD_PAL:
717 case STD_SECAM:
718 frameinterval->numerator = 1;
719 frameinterval->denominator = 25;
720 break;
721 default:
722 dev_warn(&dev->pci->dev, "tw5864_frameinterval_get requested for unknown std %d\n",
723 input->std);
724 return -EINVAL;
725 }
726
727 return 0;
728 }
729
tw5864_enum_framesizes(struct file * file,void * priv,struct v4l2_frmsizeenum * fsize)730 static int tw5864_enum_framesizes(struct file *file, void *priv,
731 struct v4l2_frmsizeenum *fsize)
732 {
733 struct tw5864_input *input = video_drvdata(file);
734
735 if (fsize->index > 0)
736 return -EINVAL;
737 if (fsize->pixel_format != V4L2_PIX_FMT_H264)
738 return -EINVAL;
739
740 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
741 fsize->discrete.width = 704;
742 fsize->discrete.height = input->std == STD_NTSC ? 480 : 576;
743
744 return 0;
745 }
746
tw5864_enum_frameintervals(struct file * file,void * priv,struct v4l2_frmivalenum * fintv)747 static int tw5864_enum_frameintervals(struct file *file, void *priv,
748 struct v4l2_frmivalenum *fintv)
749 {
750 struct tw5864_input *input = video_drvdata(file);
751 struct v4l2_fract frameinterval;
752 int std_max_fps = input->std == STD_NTSC ? 30 : 25;
753 struct v4l2_frmsizeenum fsize = { .index = fintv->index,
754 .pixel_format = fintv->pixel_format };
755 int ret;
756
757 ret = tw5864_enum_framesizes(file, priv, &fsize);
758 if (ret)
759 return ret;
760
761 if (fintv->width != fsize.discrete.width ||
762 fintv->height != fsize.discrete.height)
763 return -EINVAL;
764
765 fintv->type = V4L2_FRMIVAL_TYPE_STEPWISE;
766
767 ret = tw5864_frameinterval_get(input, &frameinterval);
768 if (ret)
769 return ret;
770
771 fintv->stepwise.step = frameinterval;
772 fintv->stepwise.min = frameinterval;
773 fintv->stepwise.max = frameinterval;
774 fintv->stepwise.max.numerator *= std_max_fps;
775
776 return ret;
777 }
778
tw5864_g_parm(struct file * file,void * priv,struct v4l2_streamparm * sp)779 static int tw5864_g_parm(struct file *file, void *priv,
780 struct v4l2_streamparm *sp)
781 {
782 struct tw5864_input *input = video_drvdata(file);
783 struct v4l2_captureparm *cp = &sp->parm.capture;
784 int ret;
785
786 cp->capability = V4L2_CAP_TIMEPERFRAME;
787
788 ret = tw5864_frameinterval_get(input, &cp->timeperframe);
789 if (ret)
790 return ret;
791
792 cp->timeperframe.numerator *= input->frame_interval;
793 cp->capturemode = 0;
794 cp->readbuffers = 2;
795
796 return ret;
797 }
798
tw5864_s_parm(struct file * file,void * priv,struct v4l2_streamparm * sp)799 static int tw5864_s_parm(struct file *file, void *priv,
800 struct v4l2_streamparm *sp)
801 {
802 struct tw5864_input *input = video_drvdata(file);
803 struct v4l2_fract *t = &sp->parm.capture.timeperframe;
804 struct v4l2_fract time_base;
805 int ret;
806
807 ret = tw5864_frameinterval_get(input, &time_base);
808 if (ret)
809 return ret;
810
811 if (!t->numerator || !t->denominator) {
812 t->numerator = time_base.numerator * input->frame_interval;
813 t->denominator = time_base.denominator;
814 } else if (t->denominator != time_base.denominator) {
815 t->numerator = t->numerator * time_base.denominator /
816 t->denominator;
817 t->denominator = time_base.denominator;
818 }
819
820 input->frame_interval = t->numerator / time_base.numerator;
821 if (input->frame_interval < 1)
822 input->frame_interval = 1;
823 tw5864_frame_interval_set(input);
824 return tw5864_g_parm(file, priv, sp);
825 }
826
827 static const struct v4l2_ctrl_ops tw5864_ctrl_ops = {
828 .s_ctrl = tw5864_s_ctrl,
829 };
830
831 static const struct v4l2_file_operations video_fops = {
832 .owner = THIS_MODULE,
833 .open = v4l2_fh_open,
834 .release = vb2_fop_release,
835 .read = vb2_fop_read,
836 .poll = vb2_fop_poll,
837 .mmap = vb2_fop_mmap,
838 .unlocked_ioctl = video_ioctl2,
839 };
840
841 #ifdef CONFIG_VIDEO_ADV_DEBUG
842
843 #define INDIR_SPACE_MAP_SHIFT 0x100000
844
tw5864_g_reg(struct file * file,void * fh,struct v4l2_dbg_register * reg)845 static int tw5864_g_reg(struct file *file, void *fh,
846 struct v4l2_dbg_register *reg)
847 {
848 struct tw5864_input *input = video_drvdata(file);
849 struct tw5864_dev *dev = input->root;
850
851 if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
852 if (reg->reg > 0x87fff)
853 return -EINVAL;
854 reg->size = 4;
855 reg->val = tw_readl(reg->reg);
856 } else {
857 __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
858
859 if (indir_addr > 0xefe)
860 return -EINVAL;
861 reg->size = 1;
862 reg->val = tw_indir_readb(reg->reg);
863 }
864 return 0;
865 }
866
tw5864_s_reg(struct file * file,void * fh,const struct v4l2_dbg_register * reg)867 static int tw5864_s_reg(struct file *file, void *fh,
868 const struct v4l2_dbg_register *reg)
869 {
870 struct tw5864_input *input = video_drvdata(file);
871 struct tw5864_dev *dev = input->root;
872
873 if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
874 if (reg->reg > 0x87fff)
875 return -EINVAL;
876 tw_writel(reg->reg, reg->val);
877 } else {
878 __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
879
880 if (indir_addr > 0xefe)
881 return -EINVAL;
882 tw_indir_writeb(reg->reg, reg->val);
883 }
884 return 0;
885 }
886 #endif
887
888 static const struct v4l2_ioctl_ops video_ioctl_ops = {
889 .vidioc_querycap = tw5864_querycap,
890 .vidioc_enum_fmt_vid_cap = tw5864_enum_fmt_vid_cap,
891 .vidioc_reqbufs = vb2_ioctl_reqbufs,
892 .vidioc_create_bufs = vb2_ioctl_create_bufs,
893 .vidioc_querybuf = vb2_ioctl_querybuf,
894 .vidioc_qbuf = vb2_ioctl_qbuf,
895 .vidioc_dqbuf = vb2_ioctl_dqbuf,
896 .vidioc_expbuf = vb2_ioctl_expbuf,
897 .vidioc_querystd = tw5864_querystd,
898 .vidioc_s_std = tw5864_s_std,
899 .vidioc_g_std = tw5864_g_std,
900 .vidioc_enum_input = tw5864_enum_input,
901 .vidioc_g_input = tw5864_g_input,
902 .vidioc_s_input = tw5864_s_input,
903 .vidioc_streamon = vb2_ioctl_streamon,
904 .vidioc_streamoff = vb2_ioctl_streamoff,
905 .vidioc_try_fmt_vid_cap = tw5864_fmt_vid_cap,
906 .vidioc_s_fmt_vid_cap = tw5864_fmt_vid_cap,
907 .vidioc_g_fmt_vid_cap = tw5864_fmt_vid_cap,
908 .vidioc_log_status = v4l2_ctrl_log_status,
909 .vidioc_subscribe_event = tw5864_subscribe_event,
910 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
911 .vidioc_enum_framesizes = tw5864_enum_framesizes,
912 .vidioc_enum_frameintervals = tw5864_enum_frameintervals,
913 .vidioc_s_parm = tw5864_s_parm,
914 .vidioc_g_parm = tw5864_g_parm,
915 #ifdef CONFIG_VIDEO_ADV_DEBUG
916 .vidioc_g_register = tw5864_g_reg,
917 .vidioc_s_register = tw5864_s_reg,
918 #endif
919 };
920
921 static const struct video_device tw5864_video_template = {
922 .name = "tw5864_video",
923 .fops = &video_fops,
924 .ioctl_ops = &video_ioctl_ops,
925 .release = video_device_release_empty,
926 .tvnorms = TW5864_NORMS,
927 .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
928 V4L2_CAP_STREAMING,
929 };
930
931 /* Motion Detection Threshold matrix */
932 static const struct v4l2_ctrl_config tw5864_md_thresholds = {
933 .ops = &tw5864_ctrl_ops,
934 .id = V4L2_CID_DETECT_MD_THRESHOLD_GRID,
935 .dims = {MD_CELLS_HOR, MD_CELLS_VERT},
936 .def = 14,
937 /* See tw5864_md_metric_from_mvd() */
938 .max = 2 * 0x0f,
939 .step = 1,
940 };
941
942 static int tw5864_video_input_init(struct tw5864_input *dev, int video_nr);
943 static void tw5864_video_input_fini(struct tw5864_input *dev);
944 static void tw5864_encoder_tables_upload(struct tw5864_dev *dev);
945
tw5864_video_init(struct tw5864_dev * dev,int * video_nr)946 int tw5864_video_init(struct tw5864_dev *dev, int *video_nr)
947 {
948 int i;
949 int ret;
950 unsigned long flags;
951 int last_dma_allocated = -1;
952 int last_input_nr_registered = -1;
953
954 for (i = 0; i < H264_BUF_CNT; i++) {
955 struct tw5864_h264_frame *frame = &dev->h264_buf[i];
956
957 frame->vlc.addr = dma_alloc_coherent(&dev->pci->dev,
958 H264_VLC_BUF_SIZE,
959 &frame->vlc.dma_addr,
960 GFP_KERNEL | GFP_DMA32);
961 if (!frame->vlc.addr) {
962 dev_err(&dev->pci->dev, "dma alloc fail\n");
963 ret = -ENOMEM;
964 goto free_dma;
965 }
966 frame->mv.addr = dma_alloc_coherent(&dev->pci->dev,
967 H264_MV_BUF_SIZE,
968 &frame->mv.dma_addr,
969 GFP_KERNEL | GFP_DMA32);
970 if (!frame->mv.addr) {
971 dev_err(&dev->pci->dev, "dma alloc fail\n");
972 ret = -ENOMEM;
973 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
974 frame->vlc.addr, frame->vlc.dma_addr);
975 goto free_dma;
976 }
977 last_dma_allocated = i;
978 }
979
980 tw5864_encoder_tables_upload(dev);
981
982 /* Picture is distorted without this block */
983 /* use falling edge to sample 54M to 108M */
984 tw_indir_writeb(TW5864_INDIR_VD_108_POL, TW5864_INDIR_VD_108_POL_BOTH);
985 tw_indir_writeb(TW5864_INDIR_CLK0_SEL, 0x00);
986
987 tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL0, 0x02);
988 tw_indir_writeb(TW5864_INDIR_DDRA_DLL_DQS_SEL1, 0x02);
989 tw_indir_writeb(TW5864_INDIR_DDRA_DLL_CLK90_SEL, 0x02);
990 tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL0, 0x02);
991 tw_indir_writeb(TW5864_INDIR_DDRB_DLL_DQS_SEL1, 0x02);
992 tw_indir_writeb(TW5864_INDIR_DDRB_DLL_CLK90_SEL, 0x02);
993
994 /* video input reset */
995 tw_indir_writeb(TW5864_INDIR_RESET, 0);
996 tw_indir_writeb(TW5864_INDIR_RESET, TW5864_INDIR_RESET_VD |
997 TW5864_INDIR_RESET_DLL | TW5864_INDIR_RESET_MUX_CORE);
998 msleep(20);
999
1000 /*
1001 * Select Part A mode for all channels.
1002 * tw_setl instead of tw_clearl for Part B mode.
1003 *
1004 * I guess "Part B" is primarily for downscaled version of same channel
1005 * which goes in Part A of same bus
1006 */
1007 tw_writel(TW5864_FULL_HALF_MODE_SEL, 0);
1008
1009 tw_indir_writeb(TW5864_INDIR_PV_VD_CK_POL,
1010 TW5864_INDIR_PV_VD_CK_POL_VD(0) |
1011 TW5864_INDIR_PV_VD_CK_POL_VD(1) |
1012 TW5864_INDIR_PV_VD_CK_POL_VD(2) |
1013 TW5864_INDIR_PV_VD_CK_POL_VD(3));
1014
1015 spin_lock_irqsave(&dev->slock, flags);
1016 dev->encoder_busy = 0;
1017 dev->h264_buf_r_index = 0;
1018 dev->h264_buf_w_index = 0;
1019 tw_writel(TW5864_VLC_STREAM_BASE_ADDR,
1020 dev->h264_buf[dev->h264_buf_w_index].vlc.dma_addr);
1021 tw_writel(TW5864_MV_STREAM_BASE_ADDR,
1022 dev->h264_buf[dev->h264_buf_w_index].mv.dma_addr);
1023 spin_unlock_irqrestore(&dev->slock, flags);
1024
1025 tw_writel(TW5864_SEN_EN_CH, 0x000f);
1026 tw_writel(TW5864_H264EN_CH_EN, 0x000f);
1027
1028 tw_writel(TW5864_H264EN_BUS0_MAP, 0x00000000);
1029 tw_writel(TW5864_H264EN_BUS1_MAP, 0x00001111);
1030 tw_writel(TW5864_H264EN_BUS2_MAP, 0x00002222);
1031 tw_writel(TW5864_H264EN_BUS3_MAP, 0x00003333);
1032
1033 /*
1034 * Quote from Intersil (manufacturer):
1035 * 0x0038 is managed by HW, and by default it won't pass the pointer set
1036 * at 0x0010. So if you don't do encoding, 0x0038 should stay at '3'
1037 * (with 4 frames in buffer). If you encode one frame and then move
1038 * 0x0010 to '1' for example, HW will take one more frame and set it to
1039 * buffer #0, and then you should see 0x0038 is set to '0'. There is
1040 * only one HW encoder engine, so 4 channels cannot get encoded
1041 * simultaneously. But each channel does have its own buffer (for
1042 * original frames and reconstructed frames). So there is no problem to
1043 * manage encoding for 4 channels at same time and no need to force
1044 * I-frames in switching channels.
1045 * End of quote.
1046 *
1047 * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0 (for any channel), we
1048 * have no "rolling" (until we change this value).
1049 * If we set 0x0010 (TW5864_ENC_BUF_PTR_REC1) to 0x3, it starts to roll
1050 * continuously together with 0x0038.
1051 */
1052 tw_writel(TW5864_ENC_BUF_PTR_REC1, 0x00ff);
1053 tw_writel(TW5864_PCI_INTTM_SCALE, 0);
1054
1055 tw_writel(TW5864_INTERLACING, TW5864_DI_EN);
1056 tw_writel(TW5864_MASTER_ENB_REG, TW5864_PCI_VLC_INTR_ENB);
1057 tw_writel(TW5864_PCI_INTR_CTL,
1058 TW5864_TIMER_INTR_ENB | TW5864_PCI_MAST_ENB |
1059 TW5864_MVD_VLC_MAST_ENB);
1060
1061 dev->irqmask |= TW5864_INTR_VLC_DONE | TW5864_INTR_TIMER;
1062 tw5864_irqmask_apply(dev);
1063
1064 INIT_WORK(&dev->bh_work, tw5864_handle_frame_work);
1065
1066 for (i = 0; i < TW5864_INPUTS; i++) {
1067 dev->inputs[i].root = dev;
1068 dev->inputs[i].nr = i;
1069 ret = tw5864_video_input_init(&dev->inputs[i], video_nr[i]);
1070 if (ret)
1071 goto fini_video_inputs;
1072 last_input_nr_registered = i;
1073 }
1074
1075 return 0;
1076
1077 fini_video_inputs:
1078 for (i = last_input_nr_registered; i >= 0; i--)
1079 tw5864_video_input_fini(&dev->inputs[i]);
1080
1081 cancel_work_sync(&dev->bh_work);
1082
1083 free_dma:
1084 for (i = last_dma_allocated; i >= 0; i--) {
1085 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1086 dev->h264_buf[i].vlc.addr,
1087 dev->h264_buf[i].vlc.dma_addr);
1088 dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1089 dev->h264_buf[i].mv.addr,
1090 dev->h264_buf[i].mv.dma_addr);
1091 }
1092
1093 return ret;
1094 }
1095
tw5864_video_input_init(struct tw5864_input * input,int video_nr)1096 static int tw5864_video_input_init(struct tw5864_input *input, int video_nr)
1097 {
1098 struct tw5864_dev *dev = input->root;
1099 int ret;
1100 struct v4l2_ctrl_handler *hdl = &input->hdl;
1101
1102 mutex_init(&input->lock);
1103 spin_lock_init(&input->slock);
1104
1105 /* setup video buffers queue */
1106 INIT_LIST_HEAD(&input->active);
1107 input->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1108 input->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1109 input->vidq.io_modes = VB2_MMAP | VB2_READ | VB2_DMABUF;
1110 input->vidq.ops = &tw5864_video_qops;
1111 input->vidq.mem_ops = &vb2_dma_contig_memops;
1112 input->vidq.drv_priv = input;
1113 input->vidq.gfp_flags = 0;
1114 input->vidq.buf_struct_size = sizeof(struct tw5864_buf);
1115 input->vidq.lock = &input->lock;
1116 input->vidq.min_queued_buffers = 2;
1117 input->vidq.dev = &input->root->pci->dev;
1118 ret = vb2_queue_init(&input->vidq);
1119 if (ret)
1120 goto free_mutex;
1121
1122 input->vdev = tw5864_video_template;
1123 input->vdev.v4l2_dev = &input->root->v4l2_dev;
1124 input->vdev.lock = &input->lock;
1125 input->vdev.queue = &input->vidq;
1126 video_set_drvdata(&input->vdev, input);
1127
1128 /* Initialize the device control structures */
1129 v4l2_ctrl_handler_init(hdl, 6);
1130 v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1131 V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1132 v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1133 V4L2_CID_CONTRAST, 0, 255, 1, 100);
1134 v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1135 V4L2_CID_SATURATION, 0, 255, 1, 128);
1136 v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_HUE, -128, 127, 1, 0);
1137 v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1138 1, MAX_GOP_SIZE, 1, GOP_SIZE);
1139 v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1140 V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 28, 51, 1, QP_VALUE);
1141 v4l2_ctrl_new_std_menu(hdl, &tw5864_ctrl_ops,
1142 V4L2_CID_DETECT_MD_MODE,
1143 V4L2_DETECT_MD_MODE_THRESHOLD_GRID, 0,
1144 V4L2_DETECT_MD_MODE_DISABLED);
1145 v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1146 V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD,
1147 tw5864_md_thresholds.min, tw5864_md_thresholds.max,
1148 tw5864_md_thresholds.step, tw5864_md_thresholds.def);
1149 input->md_threshold_grid_ctrl =
1150 v4l2_ctrl_new_custom(hdl, &tw5864_md_thresholds, NULL);
1151 if (hdl->error) {
1152 ret = hdl->error;
1153 goto free_v4l2_hdl;
1154 }
1155 input->vdev.ctrl_handler = hdl;
1156 v4l2_ctrl_handler_setup(hdl);
1157
1158 input->qp = QP_VALUE;
1159 input->gop = GOP_SIZE;
1160 input->frame_interval = 1;
1161
1162 ret = video_register_device(&input->vdev, VFL_TYPE_VIDEO, video_nr);
1163 if (ret)
1164 goto free_v4l2_hdl;
1165
1166 dev_info(&input->root->pci->dev, "Registered video device %s\n",
1167 video_device_node_name(&input->vdev));
1168
1169 /*
1170 * Set default video standard. Doesn't matter which, the detected value
1171 * will be found out by VIDIOC_QUERYSTD handler.
1172 */
1173 input->v4l2_std = V4L2_STD_NTSC_M;
1174 input->std = STD_NTSC;
1175
1176 tw_indir_writeb(TW5864_INDIR_VIN_E(video_nr), 0x07);
1177 /* to initiate auto format recognition */
1178 tw_indir_writeb(TW5864_INDIR_VIN_F(video_nr), 0xff);
1179
1180 return 0;
1181
1182 free_v4l2_hdl:
1183 v4l2_ctrl_handler_free(hdl);
1184 free_mutex:
1185 mutex_destroy(&input->lock);
1186
1187 return ret;
1188 }
1189
tw5864_video_input_fini(struct tw5864_input * dev)1190 static void tw5864_video_input_fini(struct tw5864_input *dev)
1191 {
1192 vb2_video_unregister_device(&dev->vdev);
1193 v4l2_ctrl_handler_free(&dev->hdl);
1194 }
1195
tw5864_video_fini(struct tw5864_dev * dev)1196 void tw5864_video_fini(struct tw5864_dev *dev)
1197 {
1198 int i;
1199
1200 cancel_work_sync(&dev->bh_work);
1201
1202 for (i = 0; i < TW5864_INPUTS; i++)
1203 tw5864_video_input_fini(&dev->inputs[i]);
1204
1205 for (i = 0; i < H264_BUF_CNT; i++) {
1206 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1207 dev->h264_buf[i].vlc.addr,
1208 dev->h264_buf[i].vlc.dma_addr);
1209 dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1210 dev->h264_buf[i].mv.addr,
1211 dev->h264_buf[i].mv.dma_addr);
1212 }
1213 }
1214
tw5864_prepare_frame_headers(struct tw5864_input * input)1215 void tw5864_prepare_frame_headers(struct tw5864_input *input)
1216 {
1217 struct tw5864_buf *vb = input->vb;
1218 u8 *dst;
1219 size_t dst_space;
1220 unsigned long flags;
1221
1222 if (!vb) {
1223 spin_lock_irqsave(&input->slock, flags);
1224 if (list_empty(&input->active)) {
1225 spin_unlock_irqrestore(&input->slock, flags);
1226 input->vb = NULL;
1227 return;
1228 }
1229 vb = list_first_entry(&input->active, struct tw5864_buf, list);
1230 list_del(&vb->list);
1231 spin_unlock_irqrestore(&input->slock, flags);
1232 }
1233
1234 dst = vb2_plane_vaddr(&vb->vb.vb2_buf, 0);
1235 dst_space = vb2_plane_size(&vb->vb.vb2_buf, 0);
1236
1237 /*
1238 * Low-level bitstream writing functions don't have a fine way to say
1239 * correctly that supplied buffer is too small. So we just check there
1240 * and warn, and don't care at lower level.
1241 * Currently all headers take below 32 bytes.
1242 * The buffer is supposed to have plenty of free space at this point,
1243 * anyway.
1244 */
1245 if (WARN_ON_ONCE(dst_space < 128))
1246 return;
1247
1248 /*
1249 * Generate H264 headers:
1250 * If this is first frame, put SPS and PPS
1251 */
1252 if (input->frame_gop_seqno == 0)
1253 tw5864_h264_put_stream_header(&dst, &dst_space, input->qp,
1254 input->width, input->height);
1255
1256 /* Put slice header */
1257 tw5864_h264_put_slice_header(&dst, &dst_space, input->h264_idr_pic_id,
1258 input->frame_gop_seqno,
1259 &input->tail_nb_bits, &input->tail);
1260 input->vb = vb;
1261 input->buf_cur_ptr = dst;
1262 input->buf_cur_space_left = dst_space;
1263 }
1264
1265 /*
1266 * Returns heuristic motion detection metric value from known components of
1267 * hardware-provided Motion Vector Data.
1268 */
tw5864_md_metric_from_mvd(u32 mvd)1269 static unsigned int tw5864_md_metric_from_mvd(u32 mvd)
1270 {
1271 /*
1272 * Format of motion vector data exposed by tw5864, according to
1273 * manufacturer:
1274 * mv_x 10 bits
1275 * mv_y 10 bits
1276 * non_zero_members 8 bits
1277 * mb_type 3 bits
1278 * reserved 1 bit
1279 *
1280 * non_zero_members: number of non-zero residuals in each macro block
1281 * after quantization
1282 *
1283 * unsigned int reserved = mvd >> 31;
1284 * unsigned int mb_type = (mvd >> 28) & 0x7;
1285 * unsigned int non_zero_members = (mvd >> 20) & 0xff;
1286 */
1287 unsigned int mv_y = (mvd >> 10) & 0x3ff;
1288 unsigned int mv_x = mvd & 0x3ff;
1289
1290 /* heuristic: */
1291 mv_x &= 0x0f;
1292 mv_y &= 0x0f;
1293
1294 return mv_y + mv_x;
1295 }
1296
tw5864_is_motion_triggered(struct tw5864_h264_frame * frame)1297 static int tw5864_is_motion_triggered(struct tw5864_h264_frame *frame)
1298 {
1299 struct tw5864_input *input = frame->input;
1300 u32 *mv = (u32 *)frame->mv.addr;
1301 int i;
1302 int detected = 0;
1303
1304 for (i = 0; i < MD_CELLS; i++) {
1305 const u16 thresh = input->md_threshold_grid_values[i];
1306 const unsigned int metric = tw5864_md_metric_from_mvd(mv[i]);
1307
1308 if (metric > thresh)
1309 detected = 1;
1310
1311 if (detected)
1312 break;
1313 }
1314 return detected;
1315 }
1316
tw5864_handle_frame_work(struct work_struct * t)1317 static void tw5864_handle_frame_work(struct work_struct *t)
1318 {
1319 struct tw5864_dev *dev = from_work(dev, t, bh_work);
1320 unsigned long flags;
1321 int batch_size = H264_BUF_CNT;
1322
1323 spin_lock_irqsave(&dev->slock, flags);
1324 while (dev->h264_buf_r_index != dev->h264_buf_w_index && batch_size--) {
1325 struct tw5864_h264_frame *frame =
1326 &dev->h264_buf[dev->h264_buf_r_index];
1327
1328 spin_unlock_irqrestore(&dev->slock, flags);
1329 dma_sync_single_for_cpu(&dev->pci->dev, frame->vlc.dma_addr,
1330 H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1331 dma_sync_single_for_cpu(&dev->pci->dev, frame->mv.dma_addr,
1332 H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1333 tw5864_handle_frame(frame);
1334 dma_sync_single_for_device(&dev->pci->dev, frame->vlc.dma_addr,
1335 H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1336 dma_sync_single_for_device(&dev->pci->dev, frame->mv.dma_addr,
1337 H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1338 spin_lock_irqsave(&dev->slock, flags);
1339
1340 dev->h264_buf_r_index++;
1341 dev->h264_buf_r_index %= H264_BUF_CNT;
1342 }
1343 spin_unlock_irqrestore(&dev->slock, flags);
1344 }
1345
1346 #ifdef DEBUG
tw5864_vlc_checksum(u32 * data,int len)1347 static u32 tw5864_vlc_checksum(u32 *data, int len)
1348 {
1349 u32 val, count_len = len;
1350
1351 val = *data++;
1352 while (((count_len >> 2) - 1) > 0) {
1353 val ^= *data++;
1354 count_len -= 4;
1355 }
1356 val ^= htonl((len >> 2));
1357 return val;
1358 }
1359 #endif
1360
tw5864_handle_frame(struct tw5864_h264_frame * frame)1361 static void tw5864_handle_frame(struct tw5864_h264_frame *frame)
1362 {
1363 #define SKIP_VLCBUF_BYTES 3
1364 struct tw5864_input *input = frame->input;
1365 struct tw5864_dev *dev = input->root;
1366 struct tw5864_buf *vb;
1367 struct vb2_v4l2_buffer *v4l2_buf;
1368 int frame_len = frame->vlc_len - SKIP_VLCBUF_BYTES;
1369 u8 *dst = input->buf_cur_ptr;
1370 u8 tail_mask, vlc_mask = 0;
1371 int i;
1372 u8 vlc_first_byte = ((u8 *)(frame->vlc.addr + SKIP_VLCBUF_BYTES))[0];
1373 unsigned long flags;
1374 int zero_run;
1375 u8 *src;
1376 u8 *src_end;
1377
1378 #ifdef DEBUG
1379 if (frame->checksum !=
1380 tw5864_vlc_checksum((u32 *)frame->vlc.addr, frame_len))
1381 dev_err(&dev->pci->dev,
1382 "Checksum of encoded frame doesn't match!\n");
1383 #endif
1384
1385 spin_lock_irqsave(&input->slock, flags);
1386 vb = input->vb;
1387 input->vb = NULL;
1388 spin_unlock_irqrestore(&input->slock, flags);
1389
1390 if (!vb) { /* Gone because of disabling */
1391 dev_dbg(&dev->pci->dev, "vb is empty, dropping frame\n");
1392 return;
1393 }
1394
1395 v4l2_buf = to_vb2_v4l2_buffer(&vb->vb.vb2_buf);
1396
1397 /*
1398 * Check for space.
1399 * Mind the overhead of startcode emulation prevention.
1400 */
1401 if (input->buf_cur_space_left < frame_len * 5 / 4) {
1402 dev_err_once(&dev->pci->dev,
1403 "Left space in vb2 buffer, %d bytes, is less than considered safely enough to put frame of length %d. Dropping this frame.\n",
1404 input->buf_cur_space_left, frame_len);
1405 return;
1406 }
1407
1408 for (i = 0; i < 8 - input->tail_nb_bits; i++)
1409 vlc_mask |= 1 << i;
1410 tail_mask = (~vlc_mask) & 0xff;
1411
1412 dst[0] = (input->tail & tail_mask) | (vlc_first_byte & vlc_mask);
1413 frame_len--;
1414 dst++;
1415
1416 /* H.264 startcode emulation prevention */
1417 src = frame->vlc.addr + SKIP_VLCBUF_BYTES + 1;
1418 src_end = src + frame_len;
1419 zero_run = 0;
1420 for (; src < src_end; src++) {
1421 if (zero_run < 2) {
1422 if (*src == 0)
1423 ++zero_run;
1424 else
1425 zero_run = 0;
1426 } else {
1427 if ((*src & ~0x03) == 0)
1428 *dst++ = 0x03;
1429 zero_run = *src == 0;
1430 }
1431 *dst++ = *src;
1432 }
1433
1434 vb2_set_plane_payload(&vb->vb.vb2_buf, 0,
1435 dst - (u8 *)vb2_plane_vaddr(&vb->vb.vb2_buf, 0));
1436
1437 vb->vb.vb2_buf.timestamp = frame->timestamp;
1438 v4l2_buf->field = V4L2_FIELD_INTERLACED;
1439 v4l2_buf->sequence = frame->seqno;
1440
1441 /* Check for motion flags */
1442 if (frame->gop_seqno /* P-frame */ &&
1443 tw5864_is_motion_triggered(frame)) {
1444 struct v4l2_event ev = {
1445 .type = V4L2_EVENT_MOTION_DET,
1446 .u.motion_det = {
1447 .flags = V4L2_EVENT_MD_FL_HAVE_FRAME_SEQ,
1448 .frame_sequence = v4l2_buf->sequence,
1449 },
1450 };
1451
1452 v4l2_event_queue(&input->vdev, &ev);
1453 }
1454
1455 vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
1456 }
1457
tw5864_get_v4l2_std(enum tw5864_vid_std std)1458 static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std std)
1459 {
1460 switch (std) {
1461 case STD_NTSC: return V4L2_STD_NTSC_M;
1462 case STD_PAL: return V4L2_STD_PAL_B;
1463 case STD_SECAM: return V4L2_STD_SECAM_B;
1464 case STD_NTSC443: return V4L2_STD_NTSC_443;
1465 case STD_PAL_M: return V4L2_STD_PAL_M;
1466 case STD_PAL_CN: return V4L2_STD_PAL_Nc;
1467 case STD_PAL_60: return V4L2_STD_PAL_60;
1468 case STD_INVALID: return V4L2_STD_UNKNOWN;
1469 }
1470 return 0;
1471 }
1472
tw5864_from_v4l2_std(v4l2_std_id v4l2_std)1473 static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std)
1474 {
1475 if (v4l2_std & V4L2_STD_NTSC_M)
1476 return STD_NTSC;
1477 if (v4l2_std & V4L2_STD_PAL_B)
1478 return STD_PAL;
1479 if (v4l2_std & V4L2_STD_SECAM_B)
1480 return STD_SECAM;
1481 if (v4l2_std & V4L2_STD_NTSC_443)
1482 return STD_NTSC443;
1483 if (v4l2_std & V4L2_STD_PAL_M)
1484 return STD_PAL_M;
1485 if (v4l2_std & V4L2_STD_PAL_Nc)
1486 return STD_PAL_CN;
1487 if (v4l2_std & V4L2_STD_PAL_60)
1488 return STD_PAL_60;
1489
1490 return STD_INVALID;
1491 }
1492
tw5864_encoder_tables_upload(struct tw5864_dev * dev)1493 static void tw5864_encoder_tables_upload(struct tw5864_dev *dev)
1494 {
1495 int i;
1496
1497 tw_writel(TW5864_VLC_RD, 0x1);
1498 for (i = 0; i < VLC_LOOKUP_TABLE_LEN; i++) {
1499 tw_writel((TW5864_VLC_STREAM_MEM_START + i * 4),
1500 encoder_vlc_lookup_table[i]);
1501 }
1502 tw_writel(TW5864_VLC_RD, 0x0);
1503
1504 for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1505 tw_writel((TW5864_QUAN_TAB + i * 4),
1506 forward_quantization_table[i]);
1507 }
1508
1509 for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1510 tw_writel((TW5864_QUAN_TAB + i * 4),
1511 inverse_quantization_table[i]);
1512 }
1513 }
1514