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