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