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 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 */ 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 */ 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 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 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 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 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 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 num_dst_bufs -= 3; 318 319 if (esparser_vififo_get_free_space(sess) < payload_size || 320 atomic_read(&sess->esparser_queued_bufs) >= num_dst_bufs) 321 return -EAGAIN; 322 } else if (esparser_vififo_get_free_space(sess) < payload_size) { 323 return -EAGAIN; 324 } 325 326 v4l2_m2m_src_buf_remove_by_buf(sess->m2m_ctx, vbuf); 327 328 offset = esparser_get_offset(sess); 329 330 ret = amvdec_add_ts(sess, vb->timestamp, vbuf->timecode, offset, vbuf->flags); 331 if (ret) { 332 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR); 333 return ret; 334 } 335 336 dev_dbg(core->dev, "esparser: ts = %llu pld_size = %u offset = %08X flags = %08X\n", 337 vb->timestamp, payload_size, offset, vbuf->flags); 338 339 vbuf->flags = 0; 340 vbuf->field = V4L2_FIELD_NONE; 341 vbuf->sequence = sess->sequence_out++; 342 343 if (sess->fmt_out->pixfmt == V4L2_PIX_FMT_VP9) { 344 payload_size = vp9_update_header(core, vb); 345 346 /* If unable to alter buffer to add headers */ 347 if (payload_size == 0) { 348 amvdec_remove_ts(sess, vb->timestamp); 349 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR); 350 351 return 0; 352 } 353 } 354 355 pad_size = esparser_pad_start_code(core, vb, payload_size); 356 ret = esparser_write_data(core, phy, payload_size + pad_size); 357 358 if (ret <= 0) { 359 dev_warn(core->dev, "esparser: input parsing error\n"); 360 amvdec_remove_ts(sess, vb->timestamp); 361 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR); 362 amvdec_write_parser(core, PARSER_FETCH_CMD, 0); 363 364 return 0; 365 } 366 367 atomic_inc(&sess->esparser_queued_bufs); 368 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_DONE); 369 370 return 0; 371 } 372 373 void esparser_queue_all_src(struct work_struct *work) 374 { 375 struct v4l2_m2m_buffer *buf, *n; 376 struct amvdec_session *sess = 377 container_of(work, struct amvdec_session, esparser_queue_work); 378 379 mutex_lock(&sess->lock); 380 v4l2_m2m_for_each_src_buf_safe(sess->m2m_ctx, buf, n) { 381 if (sess->should_stop) 382 break; 383 384 if (esparser_queue(sess, &buf->vb) < 0) 385 break; 386 } 387 mutex_unlock(&sess->lock); 388 } 389 390 int esparser_power_up(struct amvdec_session *sess) 391 { 392 struct amvdec_core *core = sess->core; 393 struct amvdec_ops *vdec_ops = sess->fmt_out->vdec_ops; 394 395 reset_control_reset(core->esparser_reset); 396 amvdec_write_parser(core, PARSER_CONFIG, 397 (10 << PS_CFG_PFIFO_EMPTY_CNT_BIT) | 398 (1 << PS_CFG_MAX_ES_WR_CYCLE_BIT) | 399 (16 << PS_CFG_MAX_FETCH_CYCLE_BIT)); 400 401 amvdec_write_parser(core, PFIFO_RD_PTR, 0); 402 amvdec_write_parser(core, PFIFO_WR_PTR, 0); 403 404 amvdec_write_parser(core, PARSER_SEARCH_PATTERN, 405 ES_START_CODE_PATTERN); 406 amvdec_write_parser(core, PARSER_SEARCH_MASK, ES_START_CODE_MASK); 407 408 amvdec_write_parser(core, PARSER_CONFIG, 409 (10 << PS_CFG_PFIFO_EMPTY_CNT_BIT) | 410 (1 << PS_CFG_MAX_ES_WR_CYCLE_BIT) | 411 (16 << PS_CFG_MAX_FETCH_CYCLE_BIT) | 412 (2 << PS_CFG_STARTCODE_WID_24_BIT)); 413 414 amvdec_write_parser(core, PARSER_CONTROL, 415 (ES_SEARCH | ES_PARSER_START)); 416 417 amvdec_write_parser(core, PARSER_VIDEO_START_PTR, sess->vififo_paddr); 418 amvdec_write_parser(core, PARSER_VIDEO_END_PTR, 419 sess->vififo_paddr + sess->vififo_size - 8); 420 amvdec_write_parser(core, PARSER_ES_CONTROL, 421 amvdec_read_parser(core, PARSER_ES_CONTROL) & ~1); 422 423 if (vdec_ops->conf_esparser) 424 vdec_ops->conf_esparser(sess); 425 426 amvdec_write_parser(core, PARSER_INT_STATUS, 0xffff); 427 amvdec_write_parser(core, PARSER_INT_ENABLE, 428 BIT(PARSER_INT_HOST_EN_BIT)); 429 430 return 0; 431 } 432 433 int esparser_init(struct platform_device *pdev, struct amvdec_core *core) 434 { 435 struct device *dev = &pdev->dev; 436 int ret; 437 int irq; 438 439 irq = platform_get_irq_byname(pdev, "esparser"); 440 if (irq < 0) 441 return irq; 442 443 ret = devm_request_irq(dev, irq, esparser_isr, IRQF_SHARED, 444 "esparserirq", core); 445 if (ret) { 446 dev_err(dev, "Failed requesting ESPARSER IRQ\n"); 447 return ret; 448 } 449 450 core->esparser_reset = 451 devm_reset_control_get_exclusive(dev, "esparser"); 452 if (IS_ERR(core->esparser_reset)) { 453 dev_err(dev, "Failed to get esparser_reset\n"); 454 return PTR_ERR(core->esparser_reset); 455 } 456 457 return 0; 458 } 459