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