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 
tw5864_queue_setup(struct vb2_queue * q,unsigned int * num_buffers,unsigned int * num_planes,unsigned int sizes[],struct device * alloc_ctxs[])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 
tw5864_buf_queue(struct vb2_buffer * vb)195 static void tw5864_buf_queue(struct vb2_buffer *vb)
196 {
197 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
198 	struct vb2_queue *vq = vb->vb2_queue;
199 	struct tw5864_input *dev = vb2_get_drv_priv(vq);
200 	struct tw5864_buf *buf = container_of(vbuf, struct tw5864_buf, vb);
201 	unsigned long flags;
202 
203 	spin_lock_irqsave(&dev->slock, flags);
204 	list_add_tail(&buf->list, &dev->active);
205 	spin_unlock_irqrestore(&dev->slock, flags);
206 }
207 
tw5864_input_std_get(struct tw5864_input * input,enum tw5864_vid_std * std)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 
tw5864_enable_input(struct tw5864_input * input)225 static int tw5864_enable_input(struct tw5864_input *input)
226 {
227 	struct tw5864_dev *dev = input->root;
228 	int nr = input->nr;
229 	unsigned long flags;
230 	int d1_width = 720;
231 	int d1_height;
232 	int frame_width_bus_value = 0;
233 	int frame_height_bus_value = 0;
234 	int reg_frame_bus = 0x1c;
235 	int fmt_reg_value = 0;
236 	int downscale_enabled = 0;
237 
238 	dev_dbg(&dev->pci->dev, "Enabling channel %d\n", nr);
239 
240 	input->frame_seqno = 0;
241 	input->frame_gop_seqno = 0;
242 	input->h264_idr_pic_id = 0;
243 
244 	input->reg_dsp_qp = input->qp;
245 	input->reg_dsp_ref_mvp_lambda = lambda_lookup_table[input->qp];
246 	input->reg_dsp_i4x4_weight = intra4x4_lambda3[input->qp];
247 	input->reg_emu = TW5864_EMU_EN_LPF | TW5864_EMU_EN_BHOST
248 		| TW5864_EMU_EN_SEN | TW5864_EMU_EN_ME | TW5864_EMU_EN_DDR;
249 	input->reg_dsp = nr /* channel id */
250 		| TW5864_DSP_CHROM_SW
251 		| ((0xa << 8) & TW5864_DSP_MB_DELAY)
252 		;
253 
254 	input->resolution = D1;
255 
256 	d1_height = (input->std == STD_NTSC) ? 480 : 576;
257 
258 	input->width = d1_width;
259 	input->height = d1_height;
260 
261 	input->reg_interlacing = 0x4;
262 
263 	switch (input->resolution) {
264 	case D1:
265 		frame_width_bus_value = 0x2cf;
266 		frame_height_bus_value = input->height - 1;
267 		reg_frame_bus = 0x1c;
268 		fmt_reg_value = 0;
269 		downscale_enabled = 0;
270 		input->reg_dsp_codec |= TW5864_CIF_MAP_MD | TW5864_HD1_MAP_MD;
271 		input->reg_emu |= TW5864_DSP_FRAME_TYPE_D1;
272 		input->reg_interlacing = TW5864_DI_EN | TW5864_DSP_INTER_ST;
273 
274 		tw_setl(TW5864_FULL_HALF_FLAG, 1 << nr);
275 		break;
276 	case HD1:
277 		input->height /= 2;
278 		input->width /= 2;
279 		frame_width_bus_value = 0x2cf;
280 		frame_height_bus_value = input->height * 2 - 1;
281 		reg_frame_bus = 0x1c;
282 		fmt_reg_value = 0;
283 		downscale_enabled = 0;
284 		input->reg_dsp_codec |= TW5864_HD1_MAP_MD;
285 		input->reg_emu |= TW5864_DSP_FRAME_TYPE_D1;
286 
287 		tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
288 
289 		break;
290 	case CIF:
291 		input->height /= 4;
292 		input->width /= 2;
293 		frame_width_bus_value = 0x15f;
294 		frame_height_bus_value = input->height * 2 - 1;
295 		reg_frame_bus = 0x07;
296 		fmt_reg_value = 1;
297 		downscale_enabled = 1;
298 		input->reg_dsp_codec |= TW5864_CIF_MAP_MD;
299 
300 		tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
301 		break;
302 	case QCIF:
303 		input->height /= 4;
304 		input->width /= 4;
305 		frame_width_bus_value = 0x15f;
306 		frame_height_bus_value = input->height * 2 - 1;
307 		reg_frame_bus = 0x07;
308 		fmt_reg_value = 1;
309 		downscale_enabled = 1;
310 		input->reg_dsp_codec |= TW5864_CIF_MAP_MD;
311 
312 		tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
313 		break;
314 	}
315 
316 	/* analog input width / 4 */
317 	tw_indir_writeb(TW5864_INDIR_IN_PIC_WIDTH(nr), d1_width / 4);
318 	tw_indir_writeb(TW5864_INDIR_IN_PIC_HEIGHT(nr), d1_height / 4);
319 
320 	/* output width / 4 */
321 	tw_indir_writeb(TW5864_INDIR_OUT_PIC_WIDTH(nr), input->width / 4);
322 	tw_indir_writeb(TW5864_INDIR_OUT_PIC_HEIGHT(nr), input->height / 4);
323 
324 	/*
325 	 * Crop width from 720 to 704.
326 	 * Above register settings need value 720 involved.
327 	 */
328 	input->width = 704;
329 	tw_indir_writeb(TW5864_INDIR_CROP_ETC,
330 			tw_indir_readb(TW5864_INDIR_CROP_ETC) |
331 			TW5864_INDIR_CROP_ETC_CROP_EN);
332 
333 	tw_writel(TW5864_DSP_PIC_MAX_MB,
334 		  ((input->width / 16) << 8) | (input->height / 16));
335 
336 	tw_writel(TW5864_FRAME_WIDTH_BUS_A(nr),
337 		  frame_width_bus_value);
338 	tw_writel(TW5864_FRAME_WIDTH_BUS_B(nr),
339 		  frame_width_bus_value);
340 	tw_writel(TW5864_FRAME_HEIGHT_BUS_A(nr),
341 		  frame_height_bus_value);
342 	tw_writel(TW5864_FRAME_HEIGHT_BUS_B(nr),
343 		  (frame_height_bus_value + 1) / 2 - 1);
344 
345 	tw5864_frame_interval_set(input);
346 
347 	if (downscale_enabled)
348 		tw_setl(TW5864_H264EN_CH_DNS, 1 << nr);
349 
350 	tw_mask_shift_writel(TW5864_H264EN_CH_FMT_REG1, 0x3, 2 * nr,
351 			     fmt_reg_value);
352 
353 	tw_mask_shift_writel((nr < 2
354 			      ? TW5864_H264EN_RATE_MAX_LINE_REG1
355 			      : TW5864_H264EN_RATE_MAX_LINE_REG2),
356 			     0x1f, 5 * (nr % 2),
357 			     input->std == STD_NTSC ? 29 : 24);
358 
359 	tw_mask_shift_writel((nr < 2) ? TW5864_FRAME_BUS1 :
360 			     TW5864_FRAME_BUS2, 0xff, (nr % 2) * 8,
361 			     reg_frame_bus);
362 
363 	spin_lock_irqsave(&dev->slock, flags);
364 	input->enabled = 1;
365 	spin_unlock_irqrestore(&dev->slock, flags);
366 
367 	return 0;
368 }
369 
tw5864_request_encoded_frame(struct tw5864_input * input)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 
tw5864_disable_input(struct tw5864_input * input)414 static int tw5864_disable_input(struct tw5864_input *input)
415 {
416 	struct tw5864_dev *dev = input->root;
417 	unsigned long flags;
418 
419 	dev_dbg(&dev->pci->dev, "Disabling channel %d\n", input->nr);
420 
421 	spin_lock_irqsave(&dev->slock, flags);
422 	input->enabled = 0;
423 	spin_unlock_irqrestore(&dev->slock, flags);
424 	return 0;
425 }
426 
tw5864_start_streaming(struct vb2_queue * q,unsigned int count)427 static int tw5864_start_streaming(struct vb2_queue *q, unsigned int count)
428 {
429 	struct tw5864_input *input = vb2_get_drv_priv(q);
430 	int ret;
431 
432 	ret = tw5864_enable_input(input);
433 	if (!ret)
434 		return 0;
435 
436 	while (!list_empty(&input->active)) {
437 		struct tw5864_buf *buf = list_entry(input->active.next,
438 						    struct tw5864_buf, list);
439 
440 		list_del(&buf->list);
441 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
442 	}
443 	return ret;
444 }
445 
tw5864_stop_streaming(struct vb2_queue * q)446 static void tw5864_stop_streaming(struct vb2_queue *q)
447 {
448 	unsigned long flags;
449 	struct tw5864_input *input = vb2_get_drv_priv(q);
450 
451 	tw5864_disable_input(input);
452 
453 	spin_lock_irqsave(&input->slock, flags);
454 	if (input->vb) {
455 		vb2_buffer_done(&input->vb->vb.vb2_buf, VB2_BUF_STATE_ERROR);
456 		input->vb = NULL;
457 	}
458 	while (!list_empty(&input->active)) {
459 		struct tw5864_buf *buf = list_entry(input->active.next,
460 						    struct tw5864_buf, list);
461 
462 		list_del(&buf->list);
463 		vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
464 	}
465 	spin_unlock_irqrestore(&input->slock, flags);
466 }
467 
468 static const struct vb2_ops tw5864_video_qops = {
469 	.queue_setup = tw5864_queue_setup,
470 	.buf_queue = tw5864_buf_queue,
471 	.start_streaming = tw5864_start_streaming,
472 	.stop_streaming = tw5864_stop_streaming,
473 	.wait_prepare = vb2_ops_wait_prepare,
474 	.wait_finish = vb2_ops_wait_finish,
475 };
476 
tw5864_s_ctrl(struct v4l2_ctrl * ctrl)477 static int tw5864_s_ctrl(struct v4l2_ctrl *ctrl)
478 {
479 	struct tw5864_input *input =
480 		container_of(ctrl->handler, struct tw5864_input, hdl);
481 	struct tw5864_dev *dev = input->root;
482 	unsigned long flags;
483 
484 	switch (ctrl->id) {
485 	case V4L2_CID_BRIGHTNESS:
486 		tw_indir_writeb(TW5864_INDIR_VIN_A_BRIGHT(input->nr),
487 				(u8)ctrl->val);
488 		break;
489 	case V4L2_CID_HUE:
490 		tw_indir_writeb(TW5864_INDIR_VIN_7_HUE(input->nr),
491 				(u8)ctrl->val);
492 		break;
493 	case V4L2_CID_CONTRAST:
494 		tw_indir_writeb(TW5864_INDIR_VIN_9_CNTRST(input->nr),
495 				(u8)ctrl->val);
496 		break;
497 	case V4L2_CID_SATURATION:
498 		tw_indir_writeb(TW5864_INDIR_VIN_B_SAT_U(input->nr),
499 				(u8)ctrl->val);
500 		tw_indir_writeb(TW5864_INDIR_VIN_C_SAT_V(input->nr),
501 				(u8)ctrl->val);
502 		break;
503 	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
504 		input->gop = ctrl->val;
505 		return 0;
506 	case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
507 		spin_lock_irqsave(&input->slock, flags);
508 		input->qp = ctrl->val;
509 		input->reg_dsp_qp = input->qp;
510 		input->reg_dsp_ref_mvp_lambda = lambda_lookup_table[input->qp];
511 		input->reg_dsp_i4x4_weight = intra4x4_lambda3[input->qp];
512 		spin_unlock_irqrestore(&input->slock, flags);
513 		return 0;
514 	case V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD:
515 		memset(input->md_threshold_grid_values, ctrl->val,
516 		       sizeof(input->md_threshold_grid_values));
517 		return 0;
518 	case V4L2_CID_DETECT_MD_MODE:
519 		return 0;
520 	case V4L2_CID_DETECT_MD_THRESHOLD_GRID:
521 		/* input->md_threshold_grid_ctrl->p_new.p_u16 contains data */
522 		memcpy(input->md_threshold_grid_values,
523 		       input->md_threshold_grid_ctrl->p_new.p_u16,
524 		       sizeof(input->md_threshold_grid_values));
525 		return 0;
526 	}
527 	return 0;
528 }
529 
tw5864_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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 
tw5864_enum_input(struct file * file,void * priv,struct v4l2_input * i)555 static int tw5864_enum_input(struct file *file, void *priv,
556 			     struct v4l2_input *i)
557 {
558 	struct tw5864_input *input = video_drvdata(file);
559 	struct tw5864_dev *dev = input->root;
560 
561 	u8 indir_0x000 = tw_indir_readb(TW5864_INDIR_VIN_0(input->nr));
562 	u8 indir_0x00d = tw_indir_readb(TW5864_INDIR_VIN_D(input->nr));
563 	u8 v1 = indir_0x000;
564 	u8 v2 = indir_0x00d;
565 
566 	if (i->index)
567 		return -EINVAL;
568 
569 	i->type = V4L2_INPUT_TYPE_CAMERA;
570 	snprintf(i->name, sizeof(i->name), "Encoder %d", input->nr);
571 	i->std = TW5864_NORMS;
572 	if (v1 & (1 << 7))
573 		i->status |= V4L2_IN_ST_NO_SYNC;
574 	if (!(v1 & (1 << 6)))
575 		i->status |= V4L2_IN_ST_NO_H_LOCK;
576 	if (v1 & (1 << 2))
577 		i->status |= V4L2_IN_ST_NO_SIGNAL;
578 	if (v1 & (1 << 1))
579 		i->status |= V4L2_IN_ST_NO_COLOR;
580 	if (v2 & (1 << 2))
581 		i->status |= V4L2_IN_ST_MACROVISION;
582 
583 	return 0;
584 }
585 
tw5864_g_input(struct file * file,void * priv,unsigned int * i)586 static int tw5864_g_input(struct file *file, void *priv, unsigned int *i)
587 {
588 	*i = 0;
589 	return 0;
590 }
591 
tw5864_s_input(struct file * file,void * priv,unsigned int i)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 
tw5864_querycap(struct file * file,void * priv,struct v4l2_capability * cap)599 static int tw5864_querycap(struct file *file, void *priv,
600 			   struct v4l2_capability *cap)
601 {
602 	struct tw5864_input *input = video_drvdata(file);
603 
604 	strscpy(cap->driver, "tw5864", sizeof(cap->driver));
605 	snprintf(cap->card, sizeof(cap->card), "TW5864 Encoder %d",
606 		 input->nr);
607 	return 0;
608 }
609 
tw5864_querystd(struct file * file,void * priv,v4l2_std_id * std)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, &tw_std);
617 	if (ret)
618 		return ret;
619 	*std = tw5864_get_v4l2_std(tw_std);
620 
621 	return 0;
622 }
623 
tw5864_g_std(struct file * file,void * priv,v4l2_std_id * std)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 
tw5864_s_std(struct file * file,void * priv,v4l2_std_id std)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(std);
639 	tw_indir_writeb(TW5864_INDIR_VIN_E(input->nr), input->std);
640 	return 0;
641 }
642 
tw5864_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)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 
tw5864_subscribe_event(struct v4l2_fh * fh,const struct v4l2_event_subscription * sub)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, 30, NULL);
663 	default:
664 		return v4l2_ctrl_subscribe_event(fh, sub);
665 	}
666 }
667 
tw5864_frame_interval_set(struct tw5864_input * input)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 
tw5864_frameinterval_get(struct tw5864_input * input,struct v4l2_fract * frameinterval)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 
tw5864_enum_framesizes(struct file * file,void * priv,struct v4l2_frmsizeenum * fsize)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 
tw5864_enum_frameintervals(struct file * file,void * priv,struct v4l2_frmivalenum * fintv)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);
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);
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 
tw5864_g_parm(struct file * file,void * priv,struct v4l2_streamparm * sp)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, &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 
tw5864_s_parm(struct file * file,void * priv,struct v4l2_streamparm * sp)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, &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 
tw5864_g_reg(struct file * file,void * fh,struct v4l2_dbg_register * reg)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 
tw5864_s_reg(struct file * file,void * fh,const struct v4l2_dbg_register * reg)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 
tw5864_video_init(struct tw5864_dev * dev,int * video_nr)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->pci->dev,
959 						     H264_VLC_BUF_SIZE,
960 						     &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->pci->dev,
968 						    H264_MV_BUF_SIZE,
969 						    &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->pci->dev, H264_VLC_BUF_SIZE,
975 					  frame->vlc.addr, 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(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(&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(&dev->tasklet, 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->inputs[i], 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->inputs[i]);
1081 
1082 	tasklet_kill(&dev->tasklet);
1083 
1084 free_dma:
1085 	for (i = last_dma_allocated; i >= 0; i--) {
1086 		dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1087 				  dev->h264_buf[i].vlc.addr,
1088 				  dev->h264_buf[i].vlc.dma_addr);
1089 		dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1090 				  dev->h264_buf[i].mv.addr,
1091 				  dev->h264_buf[i].mv.dma_addr);
1092 	}
1093 
1094 	return ret;
1095 }
1096 
tw5864_video_input_init(struct tw5864_input * input,int video_nr)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(&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_buffers_needed = 2;
1118 	input->vidq.dev = &input->root->pci->dev;
1119 	ret = vb2_queue_init(&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(&input->vdev, input);
1128 
1129 	/* Initialize the device control structures */
1130 	v4l2_ctrl_handler_init(hdl, 6);
1131 	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1132 			  V4L2_CID_BRIGHTNESS, -128, 127, 1, 0);
1133 	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1134 			  V4L2_CID_CONTRAST, 0, 255, 1, 100);
1135 	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1136 			  V4L2_CID_SATURATION, 0, 255, 1, 128);
1137 	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_HUE, -128, 127, 1, 0);
1138 	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1139 			  1, MAX_GOP_SIZE, 1, GOP_SIZE);
1140 	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1141 			  V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 28, 51, 1, QP_VALUE);
1142 	v4l2_ctrl_new_std_menu(hdl, &tw5864_ctrl_ops,
1143 			       V4L2_CID_DETECT_MD_MODE,
1144 			       V4L2_DETECT_MD_MODE_THRESHOLD_GRID, 0,
1145 			       V4L2_DETECT_MD_MODE_DISABLED);
1146 	v4l2_ctrl_new_std(hdl, &tw5864_ctrl_ops,
1147 			  V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD,
1148 			  tw5864_md_thresholds.min, tw5864_md_thresholds.max,
1149 			  tw5864_md_thresholds.step, tw5864_md_thresholds.def);
1150 	input->md_threshold_grid_ctrl =
1151 		v4l2_ctrl_new_custom(hdl, &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(&input->vdev, VFL_TYPE_VIDEO, 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(&input->lock);
1187 
1188 	return ret;
1189 }
1190 
tw5864_video_input_fini(struct tw5864_input * dev)1191 static void tw5864_video_input_fini(struct tw5864_input *dev)
1192 {
1193 	vb2_video_unregister_device(&dev->vdev);
1194 	v4l2_ctrl_handler_free(&dev->hdl);
1195 }
1196 
tw5864_video_fini(struct tw5864_dev * dev)1197 void tw5864_video_fini(struct tw5864_dev *dev)
1198 {
1199 	int i;
1200 
1201 	tasklet_kill(&dev->tasklet);
1202 
1203 	for (i = 0; i < TW5864_INPUTS; i++)
1204 		tw5864_video_input_fini(&dev->inputs[i]);
1205 
1206 	for (i = 0; i < H264_BUF_CNT; i++) {
1207 		dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
1208 				  dev->h264_buf[i].vlc.addr,
1209 				  dev->h264_buf[i].vlc.dma_addr);
1210 		dma_free_coherent(&dev->pci->dev, H264_MV_BUF_SIZE,
1211 				  dev->h264_buf[i].mv.addr,
1212 				  dev->h264_buf[i].mv.dma_addr);
1213 	}
1214 }
1215 
tw5864_prepare_frame_headers(struct tw5864_input * input)1216 void tw5864_prepare_frame_headers(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(&input->active)) {
1226 			spin_unlock_irqrestore(&input->slock, flags);
1227 			input->vb = NULL;
1228 			return;
1229 		}
1230 		vb = list_first_entry(&input->active, struct tw5864_buf, list);
1231 		list_del(&vb->list);
1232 		spin_unlock_irqrestore(&input->slock, flags);
1233 	}
1234 
1235 	dst = vb2_plane_vaddr(&vb->vb.vb2_buf, 0);
1236 	dst_space = vb2_plane_size(&vb->vb.vb2_buf, 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(&dst, &dst_space, input->qp,
1255 					      input->width, input->height);
1256 
1257 	/* Put slice header */
1258 	tw5864_h264_put_slice_header(&dst, &dst_space, input->h264_idr_pic_id,
1259 				     input->frame_gop_seqno,
1260 				     &input->tail_nb_bits, &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  */
tw5864_md_metric_from_mvd(u32 mvd)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 
tw5864_is_motion_triggered(struct tw5864_h264_frame * frame)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(mv[i]);
1308 
1309 		if (metric > thresh)
1310 			detected = 1;
1311 
1312 		if (detected)
1313 			break;
1314 	}
1315 	return detected;
1316 }
1317 
tw5864_handle_frame_task(struct tasklet_struct * t)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(&dev->slock, flags);
1330 		dma_sync_single_for_cpu(&dev->pci->dev, frame->vlc.dma_addr,
1331 					H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1332 		dma_sync_single_for_cpu(&dev->pci->dev, frame->mv.dma_addr,
1333 					H264_MV_BUF_SIZE, DMA_FROM_DEVICE);
1334 		tw5864_handle_frame(frame);
1335 		dma_sync_single_for_device(&dev->pci->dev, frame->vlc.dma_addr,
1336 					   H264_VLC_BUF_SIZE, DMA_FROM_DEVICE);
1337 		dma_sync_single_for_device(&dev->pci->dev, frame->mv.dma_addr,
1338 					   H264_MV_BUF_SIZE, 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(&dev->slock, flags);
1345 }
1346 
1347 #ifdef DEBUG
tw5864_vlc_checksum(u32 * data,int len)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 
tw5864_handle_frame(struct tw5864_h264_frame * frame)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(&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.vb2_buf, 0,
1436 			      dst - (u8 *)vb2_plane_vaddr(&vb->vb.vb2_buf, 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(&input->vdev, &ev);
1454 	}
1455 
1456 	vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
1457 }
1458 
tw5864_get_v4l2_std(enum tw5864_vid_std std)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 
tw5864_from_v4l2_std(v4l2_std_id v4l2_std)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 
tw5864_encoder_tables_upload(struct tw5864_dev * dev)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