1 // SPDX-License-Identifier: GPL-2.0-or-later
3 * TW5864 driver - video encoding functions
5 * Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.com>
8 #include <linux/module.h>
9 #include <media/v4l2-common.h>
10 #include <media/v4l2-event.h>
11 #include <media/videobuf2-dma-contig.h>
14 #include "tw5864-reg.h"
16 #define QUANTIZATION_TABLE_LEN 96
17 #define VLC_LOOKUP_TABLE_LEN 1024
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
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
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
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
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,
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);
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);
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[])
187 return sizes[0] < H264_VLC_BUF_SIZE ? -EINVAL : 0;
189 sizes[0] = H264_VLC_BUF_SIZE;
195 static void tw5864_buf_queue(struct vb2_buffer *vb)
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);
203 spin_lock_irqsave(&dev->slock, flags);
204 list_add_tail(&buf->list, &dev->active);
205 spin_unlock_irqrestore(&dev->slock, flags);
208 static int tw5864_input_std_get(struct tw5864_input *input,
209 enum tw5864_vid_std *std)
211 struct tw5864_dev *dev = input->root;
212 u8 std_reg = tw_indir_readb(TW5864_INDIR_VIN_E(input->nr));
214 *std = (std_reg & 0x70) >> 4;
216 if (std_reg & 0x80) {
217 dev_dbg(&dev->pci->dev,
218 "Video format detection is in progress, please wait\n");
225 static int tw5864_enable_input(struct tw5864_input *input)
227 struct tw5864_dev *dev = input->root;
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;
238 dev_dbg(&dev->pci->dev, "Enabling channel %d\n", nr);
240 input->frame_seqno = 0;
241 input->frame_gop_seqno = 0;
242 input->h264_idr_pic_id = 0;
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)
254 input->resolution = D1;
256 d1_height = (input->std == STD_NTSC) ? 480 : 576;
258 input->width = d1_width;
259 input->height = d1_height;
261 input->reg_interlacing = 0x4;
263 switch (input->resolution) {
265 frame_width_bus_value = 0x2cf;
266 frame_height_bus_value = input->height - 1;
267 reg_frame_bus = 0x1c;
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;
274 tw_setl(TW5864_FULL_HALF_FLAG, 1 << nr);
279 frame_width_bus_value = 0x2cf;
280 frame_height_bus_value = input->height * 2 - 1;
281 reg_frame_bus = 0x1c;
283 downscale_enabled = 0;
284 input->reg_dsp_codec |= TW5864_HD1_MAP_MD;
285 input->reg_emu |= TW5864_DSP_FRAME_TYPE_D1;
287 tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
293 frame_width_bus_value = 0x15f;
294 frame_height_bus_value = input->height * 2 - 1;
295 reg_frame_bus = 0x07;
297 downscale_enabled = 1;
298 input->reg_dsp_codec |= TW5864_CIF_MAP_MD;
300 tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
305 frame_width_bus_value = 0x15f;
306 frame_height_bus_value = input->height * 2 - 1;
307 reg_frame_bus = 0x07;
309 downscale_enabled = 1;
310 input->reg_dsp_codec |= TW5864_CIF_MAP_MD;
312 tw_clearl(TW5864_FULL_HALF_FLAG, 1 << nr);
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);
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);
325 * Crop width from 720 to 704.
326 * Above register settings need value 720 involved.
329 tw_indir_writeb(TW5864_INDIR_CROP_ETC,
330 tw_indir_readb(TW5864_INDIR_CROP_ETC) |
331 TW5864_INDIR_CROP_ETC_CROP_EN);
333 tw_writel(TW5864_DSP_PIC_MAX_MB,
334 ((input->width / 16) << 8) | (input->height / 16));
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);
345 tw5864_frame_interval_set(input);
347 if (downscale_enabled)
348 tw_setl(TW5864_H264EN_CH_DNS, 1 << nr);
350 tw_mask_shift_writel(TW5864_H264EN_CH_FMT_REG1, 0x3, 2 * nr,
353 tw_mask_shift_writel((nr < 2
354 ? TW5864_H264EN_RATE_MAX_LINE_REG1
355 : TW5864_H264EN_RATE_MAX_LINE_REG2),
357 input->std == STD_NTSC ? 29 : 24);
359 tw_mask_shift_writel((nr < 2) ? TW5864_FRAME_BUS1 :
360 TW5864_FRAME_BUS2, 0xff, (nr % 2) * 8,
363 spin_lock_irqsave(&dev->slock, flags);
365 spin_unlock_irqrestore(&dev->slock, flags);
370 void tw5864_request_encoded_frame(struct tw5864_input *input)
372 struct tw5864_dev *dev = input->root;
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);
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);
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;
393 /* Produce P-frame */
394 tw_writel(TW5864_MOTION_SEARCH_ETC, TW5864_INTRA_EN |
395 TW5864_ME_EN | BIT(5) /* SRCH_OPT default */);
397 tw5864_prepare_frame_headers(input);
398 tw_writel(TW5864_VLC,
400 ((input->tail_nb_bits + 24) << TW5864_VLC_BIT_ALIGN_SHIFT) |
403 enc_buf_id_new = tw_mask_shift_readl(TW5864_ENC_BUF_PTR_REC1, 0x3,
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));
410 tw_writel(TW5864_SLICE, TW5864_START_NSLICE);
411 tw_writel(TW5864_SLICE, 0);
414 static int tw5864_disable_input(struct tw5864_input *input)
416 struct tw5864_dev *dev = input->root;
419 dev_dbg(&dev->pci->dev, "Disabling channel %d\n", input->nr);
421 spin_lock_irqsave(&dev->slock, flags);
423 spin_unlock_irqrestore(&dev->slock, flags);
427 static int tw5864_start_streaming(struct vb2_queue *q, unsigned int count)
429 struct tw5864_input *input = vb2_get_drv_priv(q);
432 ret = tw5864_enable_input(input);
436 while (!list_empty(&input->active)) {
437 struct tw5864_buf *buf = list_entry(input->active.next,
438 struct tw5864_buf, list);
440 list_del(&buf->list);
441 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
446 static void tw5864_stop_streaming(struct vb2_queue *q)
449 struct tw5864_input *input = vb2_get_drv_priv(q);
451 tw5864_disable_input(input);
453 spin_lock_irqsave(&input->slock, flags);
455 vb2_buffer_done(&input->vb->vb.vb2_buf, VB2_BUF_STATE_ERROR);
458 while (!list_empty(&input->active)) {
459 struct tw5864_buf *buf = list_entry(input->active.next,
460 struct tw5864_buf, list);
462 list_del(&buf->list);
463 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
465 spin_unlock_irqrestore(&input->slock, flags);
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,
477 static int tw5864_s_ctrl(struct v4l2_ctrl *ctrl)
479 struct tw5864_input *input =
480 container_of(ctrl->handler, struct tw5864_input, hdl);
481 struct tw5864_dev *dev = input->root;
485 case V4L2_CID_BRIGHTNESS:
486 tw_indir_writeb(TW5864_INDIR_VIN_A_BRIGHT(input->nr),
490 tw_indir_writeb(TW5864_INDIR_VIN_7_HUE(input->nr),
493 case V4L2_CID_CONTRAST:
494 tw_indir_writeb(TW5864_INDIR_VIN_9_CNTRST(input->nr),
497 case V4L2_CID_SATURATION:
498 tw_indir_writeb(TW5864_INDIR_VIN_B_SAT_U(input->nr),
500 tw_indir_writeb(TW5864_INDIR_VIN_C_SAT_V(input->nr),
503 case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
504 input->gop = ctrl->val;
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);
514 case V4L2_CID_DETECT_MD_GLOBAL_THRESHOLD:
515 memset(input->md_threshold_grid_values, ctrl->val,
516 sizeof(input->md_threshold_grid_values));
518 case V4L2_CID_DETECT_MD_MODE:
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));
530 static int tw5864_fmt_vid_cap(struct file *file, void *priv,
531 struct v4l2_format *f)
533 struct tw5864_input *input = video_drvdata(file);
535 f->fmt.pix.width = 704;
536 switch (input->std) {
541 f->fmt.pix.height = 480;
545 f->fmt.pix.height = 576;
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;
555 static int tw5864_enum_input(struct file *file, void *priv,
556 struct v4l2_input *i)
558 struct tw5864_input *input = video_drvdata(file);
559 struct tw5864_dev *dev = input->root;
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));
569 i->type = V4L2_INPUT_TYPE_CAMERA;
570 snprintf(i->name, sizeof(i->name), "Encoder %d", input->nr);
571 i->std = TW5864_NORMS;
573 i->status |= V4L2_IN_ST_NO_SYNC;
574 if (!(v1 & (1 << 6)))
575 i->status |= V4L2_IN_ST_NO_H_LOCK;
577 i->status |= V4L2_IN_ST_NO_SIGNAL;
579 i->status |= V4L2_IN_ST_NO_COLOR;
581 i->status |= V4L2_IN_ST_MACROVISION;
586 static int tw5864_g_input(struct file *file, void *priv, unsigned int *i)
592 static int tw5864_s_input(struct file *file, void *priv, unsigned int i)
599 static int tw5864_querycap(struct file *file, void *priv,
600 struct v4l2_capability *cap)
602 struct tw5864_input *input = video_drvdata(file);
604 strscpy(cap->driver, "tw5864", sizeof(cap->driver));
605 snprintf(cap->card, sizeof(cap->card), "TW5864 Encoder %d",
610 static int tw5864_querystd(struct file *file, void *priv, v4l2_std_id *std)
612 struct tw5864_input *input = video_drvdata(file);
613 enum tw5864_vid_std tw_std;
616 ret = tw5864_input_std_get(input, &tw_std);
619 *std = tw5864_get_v4l2_std(tw_std);
624 static int tw5864_g_std(struct file *file, void *priv, v4l2_std_id *std)
626 struct tw5864_input *input = video_drvdata(file);
628 *std = input->v4l2_std;
632 static int tw5864_s_std(struct file *file, void *priv, v4l2_std_id std)
634 struct tw5864_input *input = video_drvdata(file);
635 struct tw5864_dev *dev = input->root;
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);
643 static int tw5864_enum_fmt_vid_cap(struct file *file, void *priv,
644 struct v4l2_fmtdesc *f)
649 f->pixelformat = V4L2_PIX_FMT_H264;
654 static int tw5864_subscribe_event(struct v4l2_fh *fh,
655 const struct v4l2_event_subscription *sub)
658 case V4L2_EVENT_MOTION_DET:
660 * Allow for up to 30 events (1 second for NTSC) to be stored.
662 return v4l2_event_subscribe(fh, sub, 30, NULL);
664 return v4l2_ctrl_subscribe_event(fh, sub);
668 static void tw5864_frame_interval_set(struct tw5864_input *input)
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.
678 * For 1 FPS - 0x00000001
679 * 00000000 00000000 00000000 00000001
681 * For max FPS - set all 25/30 lower bits:
682 * 00111111 11111111 11111111 11111111 (NTSC)
683 * 00000001 11111111 11111111 11111111 (PAL)
685 * For half of max FPS - use such pattern:
686 * 00010101 01010101 01010101 01010101 (NTSC)
687 * 00000001 01010101 01010101 01010101 (PAL)
691 * The value supplied to hardware is capped by mask of 25/30 lower bits.
693 struct tw5864_dev *dev = input->root;
694 u32 unary_framerate = 0;
696 int std_max_fps = input->std == STD_NTSC ? 30 : 25;
698 for (shift = 0; shift < std_max_fps; shift += input->frame_interval)
699 unary_framerate |= 0x00000001 << shift;
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);
707 static int tw5864_frameinterval_get(struct tw5864_input *input,
708 struct v4l2_fract *frameinterval)
710 struct tw5864_dev *dev = input->root;
712 switch (input->std) {
714 frameinterval->numerator = 1001;
715 frameinterval->denominator = 30000;
719 frameinterval->numerator = 1;
720 frameinterval->denominator = 25;
723 dev_warn(&dev->pci->dev, "tw5864_frameinterval_get requested for unknown std %d\n",
731 static int tw5864_enum_framesizes(struct file *file, void *priv,
732 struct v4l2_frmsizeenum *fsize)
734 struct tw5864_input *input = video_drvdata(file);
736 if (fsize->index > 0)
738 if (fsize->pixel_format != V4L2_PIX_FMT_H264)
741 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
742 fsize->discrete.width = 704;
743 fsize->discrete.height = input->std == STD_NTSC ? 480 : 576;
748 static int tw5864_enum_frameintervals(struct file *file, void *priv,
749 struct v4l2_frmivalenum *fintv)
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 };
758 ret = tw5864_enum_framesizes(file, priv, &fsize);
762 if (fintv->width != fsize.discrete.width ||
763 fintv->height != fsize.discrete.height)
766 fintv->type = V4L2_FRMIVAL_TYPE_STEPWISE;
768 ret = tw5864_frameinterval_get(input, &frameinterval);
772 fintv->stepwise.step = frameinterval;
773 fintv->stepwise.min = frameinterval;
774 fintv->stepwise.max = frameinterval;
775 fintv->stepwise.max.numerator *= std_max_fps;
780 static int tw5864_g_parm(struct file *file, void *priv,
781 struct v4l2_streamparm *sp)
783 struct tw5864_input *input = video_drvdata(file);
784 struct v4l2_captureparm *cp = &sp->parm.capture;
787 cp->capability = V4L2_CAP_TIMEPERFRAME;
789 ret = tw5864_frameinterval_get(input, &cp->timeperframe);
793 cp->timeperframe.numerator *= input->frame_interval;
800 static int tw5864_s_parm(struct file *file, void *priv,
801 struct v4l2_streamparm *sp)
803 struct tw5864_input *input = video_drvdata(file);
804 struct v4l2_fract *t = &sp->parm.capture.timeperframe;
805 struct v4l2_fract time_base;
808 ret = tw5864_frameinterval_get(input, &time_base);
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 /
818 t->denominator = time_base.denominator;
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);
828 static const struct v4l2_ctrl_ops tw5864_ctrl_ops = {
829 .s_ctrl = tw5864_s_ctrl,
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,
842 #ifdef CONFIG_VIDEO_ADV_DEBUG
844 #define INDIR_SPACE_MAP_SHIFT 0x100000
846 static int tw5864_g_reg(struct file *file, void *fh,
847 struct v4l2_dbg_register *reg)
849 struct tw5864_input *input = video_drvdata(file);
850 struct tw5864_dev *dev = input->root;
852 if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
853 if (reg->reg > 0x87fff)
856 reg->val = tw_readl(reg->reg);
858 __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
860 if (indir_addr > 0xefe)
863 reg->val = tw_indir_readb(reg->reg);
868 static int tw5864_s_reg(struct file *file, void *fh,
869 const struct v4l2_dbg_register *reg)
871 struct tw5864_input *input = video_drvdata(file);
872 struct tw5864_dev *dev = input->root;
874 if (reg->reg < INDIR_SPACE_MAP_SHIFT) {
875 if (reg->reg > 0x87fff)
877 tw_writel(reg->reg, reg->val);
879 __u64 indir_addr = reg->reg - INDIR_SPACE_MAP_SHIFT;
881 if (indir_addr > 0xefe)
883 tw_indir_writeb(reg->reg, reg->val);
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,
922 static const struct video_device tw5864_video_template = {
923 .name = "tw5864_video",
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 |
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},
938 /* See tw5864_md_metric_from_mvd() */
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);
947 int tw5864_video_init(struct tw5864_dev *dev, int *video_nr)
952 int last_dma_allocated = -1;
953 int last_input_nr_registered = -1;
955 for (i = 0; i < H264_BUF_CNT; i++) {
956 struct tw5864_h264_frame *frame = &dev->h264_buf[i];
958 frame->vlc.addr = dma_alloc_coherent(&dev->pci->dev,
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");
967 frame->mv.addr = dma_alloc_coherent(&dev->pci->dev,
970 GFP_KERNEL | GFP_DMA32);
971 if (!frame->mv.addr) {
972 dev_err(&dev->pci->dev, "dma alloc fail\n");
974 dma_free_coherent(&dev->pci->dev, H264_VLC_BUF_SIZE,
975 frame->vlc.addr, frame->vlc.dma_addr);
978 last_dma_allocated = i;
981 tw5864_encoder_tables_upload(dev);
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);
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);
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);
1002 * Select Part A mode for all channels.
1003 * tw_setl instead of tw_clearl for Part B mode.
1005 * I guess "Part B" is primarily for downscaled version of same channel
1006 * which goes in Part A of same bus
1008 tw_writel(TW5864_FULL_HALF_MODE_SEL, 0);
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));
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);
1026 tw_writel(TW5864_SEN_EN_CH, 0x000f);
1027 tw_writel(TW5864_H264EN_CH_EN, 0x000f);
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);
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.
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.
1053 tw_writel(TW5864_ENC_BUF_PTR_REC1, 0x00ff);
1054 tw_writel(TW5864_PCI_INTTM_SCALE, 0);
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);
1062 dev->irqmask |= TW5864_INTR_VLC_DONE | TW5864_INTR_TIMER;
1063 tw5864_irqmask_apply(dev);
1065 tasklet_setup(&dev->tasklet, tw5864_handle_frame_task);
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]);
1072 goto fini_video_inputs;
1073 last_input_nr_registered = i;
1079 for (i = last_input_nr_registered; i >= 0; i--)
1080 tw5864_video_input_fini(&dev->inputs[i]);
1082 tasklet_kill(&dev->tasklet);
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);
1097 static int tw5864_video_input_init(struct tw5864_input *input, int video_nr)
1099 struct tw5864_dev *dev = input->root;
1101 struct v4l2_ctrl_handler *hdl = &input->hdl;
1103 mutex_init(&input->lock);
1104 spin_lock_init(&input->slock);
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);
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);
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);
1156 input->vdev.ctrl_handler = hdl;
1157 v4l2_ctrl_handler_setup(hdl);
1159 input->qp = QP_VALUE;
1160 input->gop = GOP_SIZE;
1161 input->frame_interval = 1;
1163 ret = video_register_device(&input->vdev, VFL_TYPE_VIDEO, video_nr);
1167 dev_info(&input->root->pci->dev, "Registered video device %s\n",
1168 video_device_node_name(&input->vdev));
1171 * Set default video standard. Doesn't matter which, the detected value
1172 * will be found out by VIDIOC_QUERYSTD handler.
1174 input->v4l2_std = V4L2_STD_NTSC_M;
1175 input->std = STD_NTSC;
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);
1184 v4l2_ctrl_handler_free(hdl);
1186 mutex_destroy(&input->lock);
1191 static void tw5864_video_input_fini(struct tw5864_input *dev)
1193 vb2_video_unregister_device(&dev->vdev);
1194 v4l2_ctrl_handler_free(&dev->hdl);
1197 void tw5864_video_fini(struct tw5864_dev *dev)
1201 tasklet_kill(&dev->tasklet);
1203 for (i = 0; i < TW5864_INPUTS; i++)
1204 tw5864_video_input_fini(&dev->inputs[i]);
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);
1216 void tw5864_prepare_frame_headers(struct tw5864_input *input)
1218 struct tw5864_buf *vb = input->vb;
1221 unsigned long flags;
1224 spin_lock_irqsave(&input->slock, flags);
1225 if (list_empty(&input->active)) {
1226 spin_unlock_irqrestore(&input->slock, flags);
1230 vb = list_first_entry(&input->active, struct tw5864_buf, list);
1231 list_del(&vb->list);
1232 spin_unlock_irqrestore(&input->slock, flags);
1235 dst = vb2_plane_vaddr(&vb->vb.vb2_buf, 0);
1236 dst_space = vb2_plane_size(&vb->vb.vb2_buf, 0);
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,
1246 if (WARN_ON_ONCE(dst_space < 128))
1250 * Generate H264 headers:
1251 * If this is first frame, put SPS and PPS
1253 if (input->frame_gop_seqno == 0)
1254 tw5864_h264_put_stream_header(&dst, &dst_space, input->qp,
1255 input->width, input->height);
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);
1262 input->buf_cur_ptr = dst;
1263 input->buf_cur_space_left = dst_space;
1267 * Returns heuristic motion detection metric value from known components of
1268 * hardware-provided Motion Vector Data.
1270 static unsigned int tw5864_md_metric_from_mvd(u32 mvd)
1273 * Format of motion vector data exposed by tw5864, according to
1277 * non_zero_members 8 bits
1281 * non_zero_members: number of non-zero residuals in each macro block
1282 * after quantization
1284 * unsigned int reserved = mvd >> 31;
1285 * unsigned int mb_type = (mvd >> 28) & 0x7;
1286 * unsigned int non_zero_members = (mvd >> 20) & 0xff;
1288 unsigned int mv_y = (mvd >> 10) & 0x3ff;
1289 unsigned int mv_x = mvd & 0x3ff;
1298 static int tw5864_is_motion_triggered(struct tw5864_h264_frame *frame)
1300 struct tw5864_input *input = frame->input;
1301 u32 *mv = (u32 *)frame->mv.addr;
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]);
1309 if (metric > thresh)
1318 static void tw5864_handle_frame_task(struct tasklet_struct *t)
1320 struct tw5864_dev *dev = from_tasklet(dev, t, tasklet);
1321 unsigned long flags;
1322 int batch_size = H264_BUF_CNT;
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];
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);
1341 dev->h264_buf_r_index++;
1342 dev->h264_buf_r_index %= H264_BUF_CNT;
1344 spin_unlock_irqrestore(&dev->slock, flags);
1348 static u32 tw5864_vlc_checksum(u32 *data, int len)
1350 u32 val, count_len = len;
1353 while (((count_len >> 2) - 1) > 0) {
1357 val ^= htonl((len >> 2));
1362 static void tw5864_handle_frame(struct tw5864_h264_frame *frame)
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;
1373 u8 vlc_first_byte = ((u8 *)(frame->vlc.addr + SKIP_VLCBUF_BYTES))[0];
1374 unsigned long flags;
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");
1386 spin_lock_irqsave(&input->slock, flags);
1389 spin_unlock_irqrestore(&input->slock, flags);
1391 if (!vb) { /* Gone because of disabling */
1392 dev_dbg(&dev->pci->dev, "vb is empty, dropping frame\n");
1396 v4l2_buf = to_vb2_v4l2_buffer(&vb->vb.vb2_buf);
1400 * Mind the overhead of startcode emulation prevention.
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);
1409 for (i = 0; i < 8 - input->tail_nb_bits; i++)
1411 tail_mask = (~vlc_mask) & 0xff;
1413 dst[0] = (input->tail & tail_mask) | (vlc_first_byte & vlc_mask);
1417 /* H.264 startcode emulation prevention */
1418 src = frame->vlc.addr + SKIP_VLCBUF_BYTES + 1;
1419 src_end = src + frame_len;
1421 for (; src < src_end; src++) {
1428 if ((*src & ~0x03) == 0)
1430 zero_run = *src == 0;
1435 vb2_set_plane_payload(&vb->vb.vb2_buf, 0,
1436 dst - (u8 *)vb2_plane_vaddr(&vb->vb.vb2_buf, 0));
1438 vb->vb.vb2_buf.timestamp = frame->timestamp;
1439 v4l2_buf->field = V4L2_FIELD_INTERLACED;
1440 v4l2_buf->sequence = frame->seqno;
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,
1448 .flags = V4L2_EVENT_MD_FL_HAVE_FRAME_SEQ,
1449 .frame_sequence = v4l2_buf->sequence,
1453 v4l2_event_queue(&input->vdev, &ev);
1456 vb2_buffer_done(&vb->vb.vb2_buf, VB2_BUF_STATE_DONE);
1459 static v4l2_std_id tw5864_get_v4l2_std(enum tw5864_vid_std 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;
1474 static enum tw5864_vid_std tw5864_from_v4l2_std(v4l2_std_id v4l2_std)
1476 if (v4l2_std & V4L2_STD_NTSC_M)
1478 if (v4l2_std & V4L2_STD_PAL_B)
1480 if (v4l2_std & V4L2_STD_SECAM_B)
1482 if (v4l2_std & V4L2_STD_NTSC_443)
1484 if (v4l2_std & V4L2_STD_PAL_M)
1486 if (v4l2_std & V4L2_STD_PAL_Nc)
1488 if (v4l2_std & V4L2_STD_PAL_60)
1494 static void tw5864_encoder_tables_upload(struct tw5864_dev *dev)
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]);
1503 tw_writel(TW5864_VLC_RD, 0x0);
1505 for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1506 tw_writel((TW5864_QUAN_TAB + i * 4),
1507 forward_quantization_table[i]);
1510 for (i = 0; i < QUANTIZATION_TABLE_LEN; i++) {
1511 tw_writel((TW5864_QUAN_TAB + i * 4),
1512 inverse_quantization_table[i]);