1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * Copyright (C) 2018 BayLibre, SAS
4 * Author: Maxime Jourdan <mjourdan@baylibre.com>
5 *
6 * The Elementary Stream Parser is a HW bitstream parser.
7 * It reads bitstream buffers and feeds them to the VIFIFO
8 */
9
10 #include <linux/init.h>
11 #include <linux/ioctl.h>
12 #include <linux/list.h>
13 #include <linux/module.h>
14 #include <linux/reset.h>
15 #include <linux/interrupt.h>
16 #include <media/videobuf2-dma-contig.h>
17 #include <media/v4l2-mem2mem.h>
18
19 #include "dos_regs.h"
20 #include "esparser.h"
21 #include "vdec_helpers.h"
22
23 /* PARSER REGS (CBUS) */
24 #define PARSER_CONTROL 0x00
25 #define ES_PACK_SIZE_BIT 8
26 #define ES_WRITE BIT(5)
27 #define ES_SEARCH BIT(1)
28 #define ES_PARSER_START BIT(0)
29 #define PARSER_FETCH_ADDR 0x4
30 #define PARSER_FETCH_CMD 0x8
31 #define PARSER_CONFIG 0x14
32 #define PS_CFG_MAX_FETCH_CYCLE_BIT 0
33 #define PS_CFG_STARTCODE_WID_24_BIT 10
34 #define PS_CFG_MAX_ES_WR_CYCLE_BIT 12
35 #define PS_CFG_PFIFO_EMPTY_CNT_BIT 16
36 #define PFIFO_WR_PTR 0x18
37 #define PFIFO_RD_PTR 0x1c
38 #define PARSER_SEARCH_PATTERN 0x24
39 #define ES_START_CODE_PATTERN 0x00000100
40 #define PARSER_SEARCH_MASK 0x28
41 #define ES_START_CODE_MASK 0xffffff00
42 #define FETCH_ENDIAN_BIT 27
43 #define PARSER_INT_ENABLE 0x2c
44 #define PARSER_INT_HOST_EN_BIT 8
45 #define PARSER_INT_STATUS 0x30
46 #define PARSER_INTSTAT_SC_FOUND 1
47 #define PARSER_ES_CONTROL 0x5c
48 #define PARSER_VIDEO_START_PTR 0x80
49 #define PARSER_VIDEO_END_PTR 0x84
50 #define PARSER_VIDEO_WP 0x88
51 #define PARSER_VIDEO_HOLE 0x90
52
53 #define SEARCH_PATTERN_LEN 512
54 #define VP9_HEADER_SIZE 16
55
56 static DECLARE_WAIT_QUEUE_HEAD(wq);
57 static int search_done;
58
esparser_isr(int irq,void * dev)59 static irqreturn_t esparser_isr(int irq, void *dev)
60 {
61 int int_status;
62 struct amvdec_core *core = dev;
63
64 int_status = amvdec_read_parser(core, PARSER_INT_STATUS);
65 amvdec_write_parser(core, PARSER_INT_STATUS, int_status);
66
67 if (int_status & PARSER_INTSTAT_SC_FOUND) {
68 amvdec_write_parser(core, PFIFO_RD_PTR, 0);
69 amvdec_write_parser(core, PFIFO_WR_PTR, 0);
70 search_done = 1;
71 wake_up_interruptible(&wq);
72 }
73
74 return IRQ_HANDLED;
75 }
76
77 /*
78 * VP9 frame headers need to be appended by a 16-byte long
79 * Amlogic custom header
80 */
vp9_update_header(struct amvdec_core * core,struct vb2_buffer * buf)81 static int vp9_update_header(struct amvdec_core *core, struct vb2_buffer *buf)
82 {
83 u8 *dp;
84 u8 marker;
85 int dsize;
86 int num_frames, cur_frame;
87 int cur_mag, mag, mag_ptr;
88 int frame_size[8], tot_frame_size[8];
89 int total_datasize = 0;
90 int new_frame_size;
91 unsigned char *old_header = NULL;
92
93 dp = (uint8_t *)vb2_plane_vaddr(buf, 0);
94 dsize = vb2_get_plane_payload(buf, 0);
95
96 if (dsize == vb2_plane_size(buf, 0)) {
97 dev_warn(core->dev, "%s: unable to update header\n", __func__);
98 return 0;
99 }
100
101 marker = dp[dsize - 1];
102 if ((marker & 0xe0) == 0xc0) {
103 num_frames = (marker & 0x7) + 1;
104 mag = ((marker >> 3) & 0x3) + 1;
105 mag_ptr = dsize - mag * num_frames - 2;
106 if (dp[mag_ptr] != marker)
107 return 0;
108
109 mag_ptr++;
110 for (cur_frame = 0; cur_frame < num_frames; cur_frame++) {
111 frame_size[cur_frame] = 0;
112 for (cur_mag = 0; cur_mag < mag; cur_mag++) {
113 frame_size[cur_frame] |=
114 (dp[mag_ptr] << (cur_mag * 8));
115 mag_ptr++;
116 }
117 if (cur_frame == 0)
118 tot_frame_size[cur_frame] =
119 frame_size[cur_frame];
120 else
121 tot_frame_size[cur_frame] =
122 tot_frame_size[cur_frame - 1] +
123 frame_size[cur_frame];
124 total_datasize += frame_size[cur_frame];
125 }
126 } else {
127 num_frames = 1;
128 frame_size[0] = dsize;
129 tot_frame_size[0] = dsize;
130 total_datasize = dsize;
131 }
132
133 new_frame_size = total_datasize + num_frames * VP9_HEADER_SIZE;
134
135 if (new_frame_size >= vb2_plane_size(buf, 0)) {
136 dev_warn(core->dev, "%s: unable to update header\n", __func__);
137 return 0;
138 }
139
140 for (cur_frame = num_frames - 1; cur_frame >= 0; cur_frame--) {
141 int framesize = frame_size[cur_frame];
142 int framesize_header = framesize + 4;
143 int oldframeoff = tot_frame_size[cur_frame] - framesize;
144 int outheaderoff = oldframeoff + cur_frame * VP9_HEADER_SIZE;
145 u8 *fdata = dp + outheaderoff;
146 u8 *old_framedata = dp + oldframeoff;
147
148 memmove(fdata + VP9_HEADER_SIZE, old_framedata, framesize);
149
150 fdata[0] = (framesize_header >> 24) & 0xff;
151 fdata[1] = (framesize_header >> 16) & 0xff;
152 fdata[2] = (framesize_header >> 8) & 0xff;
153 fdata[3] = (framesize_header >> 0) & 0xff;
154 fdata[4] = ((framesize_header >> 24) & 0xff) ^ 0xff;
155 fdata[5] = ((framesize_header >> 16) & 0xff) ^ 0xff;
156 fdata[6] = ((framesize_header >> 8) & 0xff) ^ 0xff;
157 fdata[7] = ((framesize_header >> 0) & 0xff) ^ 0xff;
158 fdata[8] = 0;
159 fdata[9] = 0;
160 fdata[10] = 0;
161 fdata[11] = 1;
162 fdata[12] = 'A';
163 fdata[13] = 'M';
164 fdata[14] = 'L';
165 fdata[15] = 'V';
166
167 if (!old_header) {
168 /* nothing */
169 } else if (old_header > fdata + 16 + framesize) {
170 dev_dbg(core->dev, "%s: data has gaps, setting to 0\n",
171 __func__);
172 memset(fdata + 16 + framesize, 0,
173 (old_header - fdata + 16 + framesize));
174 } else if (old_header < fdata + 16 + framesize) {
175 dev_err(core->dev, "%s: data overwritten\n", __func__);
176 }
177 old_header = fdata;
178 }
179
180 return new_frame_size;
181 }
182
183 /* Pad the packet to at least 4KiB bytes otherwise the VDEC unit won't trigger
184 * ISRs.
185 * Also append a start code 000001ff at the end to trigger
186 * the ESPARSER interrupt.
187 */
esparser_pad_start_code(struct amvdec_core * core,struct vb2_buffer * vb,u32 payload_size)188 static u32 esparser_pad_start_code(struct amvdec_core *core,
189 struct vb2_buffer *vb,
190 u32 payload_size)
191 {
192 u32 pad_size = 0;
193 u8 *vaddr = vb2_plane_vaddr(vb, 0);
194
195 if (payload_size < ESPARSER_MIN_PACKET_SIZE) {
196 pad_size = ESPARSER_MIN_PACKET_SIZE - payload_size;
197 memset(vaddr + payload_size, 0, pad_size);
198 }
199
200 if ((payload_size + pad_size + SEARCH_PATTERN_LEN) >
201 vb2_plane_size(vb, 0)) {
202 dev_warn(core->dev, "%s: unable to pad start code\n", __func__);
203 return pad_size;
204 }
205
206 memset(vaddr + payload_size + pad_size, 0, SEARCH_PATTERN_LEN);
207 vaddr[payload_size + pad_size] = 0x00;
208 vaddr[payload_size + pad_size + 1] = 0x00;
209 vaddr[payload_size + pad_size + 2] = 0x01;
210 vaddr[payload_size + pad_size + 3] = 0xff;
211
212 return pad_size;
213 }
214
215 static int
esparser_write_data(struct amvdec_core * core,dma_addr_t addr,u32 size)216 esparser_write_data(struct amvdec_core *core, dma_addr_t addr, u32 size)
217 {
218 amvdec_write_parser(core, PFIFO_RD_PTR, 0);
219 amvdec_write_parser(core, PFIFO_WR_PTR, 0);
220 amvdec_write_parser(core, PARSER_CONTROL,
221 ES_WRITE |
222 ES_PARSER_START |
223 ES_SEARCH |
224 (size << ES_PACK_SIZE_BIT));
225
226 amvdec_write_parser(core, PARSER_FETCH_ADDR, addr);
227 amvdec_write_parser(core, PARSER_FETCH_CMD,
228 (7 << FETCH_ENDIAN_BIT) |
229 (size + SEARCH_PATTERN_LEN));
230
231 search_done = 0;
232 return wait_event_interruptible_timeout(wq, search_done, (HZ / 5));
233 }
234
esparser_vififo_get_free_space(struct amvdec_session * sess)235 static u32 esparser_vififo_get_free_space(struct amvdec_session *sess)
236 {
237 u32 vififo_usage;
238 struct amvdec_ops *vdec_ops = sess->fmt_out->vdec_ops;
239 struct amvdec_core *core = sess->core;
240
241 vififo_usage = vdec_ops->vififo_level(sess);
242 vififo_usage += amvdec_read_parser(core, PARSER_VIDEO_HOLE);
243 vififo_usage += (6 * SZ_1K); // 6 KiB internal fifo
244
245 if (vififo_usage > sess->vififo_size) {
246 dev_warn(sess->core->dev,
247 "VIFIFO usage (%u) > VIFIFO size (%u)\n",
248 vififo_usage, sess->vififo_size);
249 return 0;
250 }
251
252 return sess->vififo_size - vififo_usage;
253 }
254
esparser_queue_eos(struct amvdec_core * core,const u8 * data,u32 len)255 int esparser_queue_eos(struct amvdec_core *core, const u8 *data, u32 len)
256 {
257 struct device *dev = core->dev;
258 void *eos_vaddr;
259 dma_addr_t eos_paddr;
260 int ret;
261
262 eos_vaddr = dma_alloc_coherent(dev, len + SEARCH_PATTERN_LEN,
263 &eos_paddr, GFP_KERNEL);
264 if (!eos_vaddr)
265 return -ENOMEM;
266
267 memcpy(eos_vaddr, data, len);
268 ret = esparser_write_data(core, eos_paddr, len);
269 dma_free_coherent(dev, len + SEARCH_PATTERN_LEN,
270 eos_vaddr, eos_paddr);
271
272 return ret;
273 }
274
esparser_get_offset(struct amvdec_session * sess)275 static u32 esparser_get_offset(struct amvdec_session *sess)
276 {
277 struct amvdec_core *core = sess->core;
278 u32 offset = amvdec_read_parser(core, PARSER_VIDEO_WP) -
279 sess->vififo_paddr;
280
281 if (offset < sess->last_offset)
282 sess->wrap_count++;
283
284 sess->last_offset = offset;
285 offset += (sess->wrap_count * sess->vififo_size);
286
287 return offset;
288 }
289
290 static int
esparser_queue(struct amvdec_session * sess,struct vb2_v4l2_buffer * vbuf)291 esparser_queue(struct amvdec_session *sess, struct vb2_v4l2_buffer *vbuf)
292 {
293 int ret;
294 struct vb2_buffer *vb = &vbuf->vb2_buf;
295 struct amvdec_core *core = sess->core;
296 struct amvdec_codec_ops *codec_ops = sess->fmt_out->codec_ops;
297 u32 payload_size = vb2_get_plane_payload(vb, 0);
298 dma_addr_t phy = vb2_dma_contig_plane_dma_addr(vb, 0);
299 u32 num_dst_bufs = 0;
300 u32 offset;
301 u32 pad_size;
302
303 /*
304 * When max ref frame is held by VP9, this should be -= 3 to prevent a
305 * shortage of CAPTURE buffers on the decoder side.
306 * For the future, a good enhancement of the way this is handled could
307 * be to notify new capture buffers to the decoding modules, so that
308 * they could pause when there is no capture buffer available and
309 * resume on this notification.
310 */
311 if (sess->fmt_out->pixfmt == V4L2_PIX_FMT_VP9) {
312 if (codec_ops->num_pending_bufs)
313 num_dst_bufs = codec_ops->num_pending_bufs(sess);
314
315 num_dst_bufs += v4l2_m2m_num_dst_bufs_ready(sess->m2m_ctx);
316 num_dst_bufs -= 3;
317
318 if (esparser_vififo_get_free_space(sess) < payload_size ||
319 atomic_read(&sess->esparser_queued_bufs) >= num_dst_bufs)
320 return -EAGAIN;
321 } else if (esparser_vififo_get_free_space(sess) < payload_size) {
322 return -EAGAIN;
323 }
324
325 v4l2_m2m_src_buf_remove_by_buf(sess->m2m_ctx, vbuf);
326
327 offset = esparser_get_offset(sess);
328
329 ret = amvdec_add_ts(sess, vb->timestamp, vbuf->timecode, offset, vbuf->flags);
330 if (ret) {
331 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
332 return ret;
333 }
334
335 dev_dbg(core->dev, "esparser: ts = %llu pld_size = %u offset = %08X flags = %08X\n",
336 vb->timestamp, payload_size, offset, vbuf->flags);
337
338 vbuf->flags = 0;
339 vbuf->field = V4L2_FIELD_NONE;
340 vbuf->sequence = sess->sequence_out++;
341
342 if (sess->fmt_out->pixfmt == V4L2_PIX_FMT_VP9) {
343 payload_size = vp9_update_header(core, vb);
344
345 /* If unable to alter buffer to add headers */
346 if (payload_size == 0) {
347 amvdec_remove_ts(sess, vb->timestamp);
348 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
349
350 return 0;
351 }
352 }
353
354 pad_size = esparser_pad_start_code(core, vb, payload_size);
355 ret = esparser_write_data(core, phy, payload_size + pad_size);
356
357 if (ret <= 0) {
358 dev_warn(core->dev, "esparser: input parsing error\n");
359 amvdec_remove_ts(sess, vb->timestamp);
360 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
361 amvdec_write_parser(core, PARSER_FETCH_CMD, 0);
362
363 return 0;
364 }
365
366 atomic_inc(&sess->esparser_queued_bufs);
367 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_DONE);
368
369 return 0;
370 }
371
esparser_queue_all_src(struct work_struct * work)372 void esparser_queue_all_src(struct work_struct *work)
373 {
374 struct v4l2_m2m_buffer *buf, *n;
375 struct amvdec_session *sess =
376 container_of(work, struct amvdec_session, esparser_queue_work);
377
378 mutex_lock(&sess->lock);
379 v4l2_m2m_for_each_src_buf_safe(sess->m2m_ctx, buf, n) {
380 if (sess->should_stop)
381 break;
382
383 if (esparser_queue(sess, &buf->vb) < 0)
384 break;
385 }
386 mutex_unlock(&sess->lock);
387 }
388
esparser_power_up(struct amvdec_session * sess)389 int esparser_power_up(struct amvdec_session *sess)
390 {
391 struct amvdec_core *core = sess->core;
392 struct amvdec_ops *vdec_ops = sess->fmt_out->vdec_ops;
393
394 reset_control_reset(core->esparser_reset);
395 amvdec_write_parser(core, PARSER_CONFIG,
396 (10 << PS_CFG_PFIFO_EMPTY_CNT_BIT) |
397 (1 << PS_CFG_MAX_ES_WR_CYCLE_BIT) |
398 (16 << PS_CFG_MAX_FETCH_CYCLE_BIT));
399
400 amvdec_write_parser(core, PFIFO_RD_PTR, 0);
401 amvdec_write_parser(core, PFIFO_WR_PTR, 0);
402
403 amvdec_write_parser(core, PARSER_SEARCH_PATTERN,
404 ES_START_CODE_PATTERN);
405 amvdec_write_parser(core, PARSER_SEARCH_MASK, ES_START_CODE_MASK);
406
407 amvdec_write_parser(core, PARSER_CONFIG,
408 (10 << PS_CFG_PFIFO_EMPTY_CNT_BIT) |
409 (1 << PS_CFG_MAX_ES_WR_CYCLE_BIT) |
410 (16 << PS_CFG_MAX_FETCH_CYCLE_BIT) |
411 (2 << PS_CFG_STARTCODE_WID_24_BIT));
412
413 amvdec_write_parser(core, PARSER_CONTROL,
414 (ES_SEARCH | ES_PARSER_START));
415
416 amvdec_write_parser(core, PARSER_VIDEO_START_PTR, sess->vififo_paddr);
417 amvdec_write_parser(core, PARSER_VIDEO_END_PTR,
418 sess->vififo_paddr + sess->vififo_size - 8);
419 amvdec_write_parser(core, PARSER_ES_CONTROL,
420 amvdec_read_parser(core, PARSER_ES_CONTROL) & ~1);
421
422 if (vdec_ops->conf_esparser)
423 vdec_ops->conf_esparser(sess);
424
425 amvdec_write_parser(core, PARSER_INT_STATUS, 0xffff);
426 amvdec_write_parser(core, PARSER_INT_ENABLE,
427 BIT(PARSER_INT_HOST_EN_BIT));
428
429 return 0;
430 }
431
esparser_init(struct platform_device * pdev,struct amvdec_core * core)432 int esparser_init(struct platform_device *pdev, struct amvdec_core *core)
433 {
434 struct device *dev = &pdev->dev;
435 int ret;
436 int irq;
437
438 irq = platform_get_irq_byname(pdev, "esparser");
439 if (irq < 0)
440 return irq;
441
442 ret = devm_request_irq(dev, irq, esparser_isr, IRQF_SHARED,
443 "esparserirq", core);
444 if (ret) {
445 dev_err(dev, "Failed requesting ESPARSER IRQ\n");
446 return ret;
447 }
448
449 core->esparser_reset =
450 devm_reset_control_get_exclusive(dev, "esparser");
451 if (IS_ERR(core->esparser_reset)) {
452 dev_err(dev, "Failed to get esparser_reset\n");
453 return PTR_ERR(core->esparser_reset);
454 }
455
456 return 0;
457 }
458