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