• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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/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 /* PARSER REGS (CBUS) */
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 
57 static DECLARE_WAIT_QUEUE_HEAD(wq);
58 static int search_done;
59 
esparser_isr(int irq,void * dev)60 static 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  * VP9 frame headers need to be appended by a 16-byte long
80  * Amlogic custom header
81  */
vp9_update_header(struct amvdec_core * core,struct vb2_buffer * buf)82 static 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 			/* nothing */
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 /* Pad the packet to at least 4KiB bytes otherwise the VDEC unit won't trigger
185  * ISRs.
186  * Also append a start code 000001ff at the end to trigger
187  * the ESPARSER interrupt.
188  */
esparser_pad_start_code(struct amvdec_core * core,struct vb2_buffer * vb,u32 payload_size)189 static 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 
216 static int
esparser_write_data(struct amvdec_core * core,dma_addr_t addr,u32 size)217 esparser_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 
esparser_vififo_get_free_space(struct amvdec_session * sess)236 static 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); // 6 KiB internal fifo
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 
esparser_queue_eos(struct amvdec_core * core,const u8 * data,u32 len)256 int 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 
esparser_get_offset(struct amvdec_session * sess)276 static 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 
291 static int
esparser_queue(struct amvdec_session * sess,struct vb2_v4l2_buffer * vbuf)292 esparser_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 	 * When max ref frame is held by VP9, this should be -= 3 to prevent a
306 	 * shortage of CAPTURE buffers on the decoder side.
307 	 * For the future, a good enhancement of the way this is handled could
308 	 * be to notify new capture buffers to the decoding modules, so that
309 	 * they could pause when there is no capture buffer available and
310 	 * resume on this notification.
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 	ret = amvdec_add_ts(sess, vb->timestamp, vbuf->timecode, offset, vbuf->flags);
332 	if (ret) {
333 		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
334 		return ret;
335 	}
336 
337 	dev_dbg(core->dev, "esparser: ts = %llu pld_size = %u offset = %08X flags = %08X\n",
338 		vb->timestamp, payload_size, offset, vbuf->flags);
339 
340 	vbuf->flags = 0;
341 	vbuf->field = V4L2_FIELD_NONE;
342 	vbuf->sequence = sess->sequence_out++;
343 
344 	if (sess->fmt_out->pixfmt == V4L2_PIX_FMT_VP9) {
345 		payload_size = vp9_update_header(core, vb);
346 
347 		/* If unable to alter buffer to add headers */
348 		if (payload_size == 0) {
349 			amvdec_remove_ts(sess, vb->timestamp);
350 			v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
351 
352 			return 0;
353 		}
354 	}
355 
356 	pad_size = esparser_pad_start_code(core, vb, payload_size);
357 	ret = esparser_write_data(core, phy, payload_size + pad_size);
358 
359 	if (ret <= 0) {
360 		dev_warn(core->dev, "esparser: input parsing error\n");
361 		amvdec_remove_ts(sess, vb->timestamp);
362 		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
363 		amvdec_write_parser(core, PARSER_FETCH_CMD, 0);
364 
365 		return 0;
366 	}
367 
368 	atomic_inc(&sess->esparser_queued_bufs);
369 	v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_DONE);
370 
371 	return 0;
372 }
373 
esparser_queue_all_src(struct work_struct * work)374 void esparser_queue_all_src(struct work_struct *work)
375 {
376 	struct v4l2_m2m_buffer *buf, *n;
377 	struct amvdec_session *sess =
378 		container_of(work, struct amvdec_session, esparser_queue_work);
379 
380 	mutex_lock(&sess->lock);
381 	v4l2_m2m_for_each_src_buf_safe(sess->m2m_ctx, buf, n) {
382 		if (sess->should_stop)
383 			break;
384 
385 		if (esparser_queue(sess, &buf->vb) < 0)
386 			break;
387 	}
388 	mutex_unlock(&sess->lock);
389 }
390 
esparser_power_up(struct amvdec_session * sess)391 int esparser_power_up(struct amvdec_session *sess)
392 {
393 	struct amvdec_core *core = sess->core;
394 	struct amvdec_ops *vdec_ops = sess->fmt_out->vdec_ops;
395 
396 	reset_control_reset(core->esparser_reset);
397 	amvdec_write_parser(core, PARSER_CONFIG,
398 			    (10 << PS_CFG_PFIFO_EMPTY_CNT_BIT) |
399 			    (1  << PS_CFG_MAX_ES_WR_CYCLE_BIT) |
400 			    (16 << PS_CFG_MAX_FETCH_CYCLE_BIT));
401 
402 	amvdec_write_parser(core, PFIFO_RD_PTR, 0);
403 	amvdec_write_parser(core, PFIFO_WR_PTR, 0);
404 
405 	amvdec_write_parser(core, PARSER_SEARCH_PATTERN,
406 			    ES_START_CODE_PATTERN);
407 	amvdec_write_parser(core, PARSER_SEARCH_MASK, ES_START_CODE_MASK);
408 
409 	amvdec_write_parser(core, PARSER_CONFIG,
410 			    (10 << PS_CFG_PFIFO_EMPTY_CNT_BIT) |
411 			    (1  << PS_CFG_MAX_ES_WR_CYCLE_BIT) |
412 			    (16 << PS_CFG_MAX_FETCH_CYCLE_BIT) |
413 			    (2  << PS_CFG_STARTCODE_WID_24_BIT));
414 
415 	amvdec_write_parser(core, PARSER_CONTROL,
416 			    (ES_SEARCH | ES_PARSER_START));
417 
418 	amvdec_write_parser(core, PARSER_VIDEO_START_PTR, sess->vififo_paddr);
419 	amvdec_write_parser(core, PARSER_VIDEO_END_PTR,
420 			    sess->vififo_paddr + sess->vififo_size - 8);
421 	amvdec_write_parser(core, PARSER_ES_CONTROL,
422 			    amvdec_read_parser(core, PARSER_ES_CONTROL) & ~1);
423 
424 	if (vdec_ops->conf_esparser)
425 		vdec_ops->conf_esparser(sess);
426 
427 	amvdec_write_parser(core, PARSER_INT_STATUS, 0xffff);
428 	amvdec_write_parser(core, PARSER_INT_ENABLE,
429 			    BIT(PARSER_INT_HOST_EN_BIT));
430 
431 	return 0;
432 }
433 
esparser_init(struct platform_device * pdev,struct amvdec_core * core)434 int esparser_init(struct platform_device *pdev, struct amvdec_core *core)
435 {
436 	struct device *dev = &pdev->dev;
437 	int ret;
438 	int irq;
439 
440 	irq = platform_get_irq_byname(pdev, "esparser");
441 	if (irq < 0)
442 		return irq;
443 
444 	ret = devm_request_irq(dev, irq, esparser_isr, IRQF_SHARED,
445 			       "esparserirq", core);
446 	if (ret) {
447 		dev_err(dev, "Failed requesting ESPARSER IRQ\n");
448 		return ret;
449 	}
450 
451 	core->esparser_reset =
452 		devm_reset_control_get_exclusive(dev, "esparser");
453 	if (IS_ERR(core->esparser_reset)) {
454 		dev_err(dev, "Failed to get esparser_reset\n");
455 		return PTR_ERR(core->esparser_reset);
456 	}
457 
458 	return 0;
459 }
460