1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Vidtv serves as a reference DVB driver and helps validate the existing APIs 4 * in the media subsystem. It can also aid developers working on userspace 5 * applications. 6 * 7 * This file contains the multiplexer logic for TS packets from different 8 * elementary streams 9 * 10 * Loosely based on libavcodec/mpegtsenc.c 11 * 12 * Copyright (C) 2020 Daniel W. S. Almeida 13 */ 14 15 #include <linux/delay.h> 16 #include <linux/dev_printk.h> 17 #include <linux/jiffies.h> 18 #include <linux/kernel.h> 19 #include <linux/math64.h> 20 #include <linux/ratelimit.h> 21 #include <linux/slab.h> 22 #include <linux/types.h> 23 #include <linux/vmalloc.h> 24 25 #include "vidtv_channel.h" 26 #include "vidtv_common.h" 27 #include "vidtv_encoder.h" 28 #include "vidtv_mux.h" 29 #include "vidtv_pes.h" 30 #include "vidtv_psi.h" 31 #include "vidtv_ts.h" 32 33 static struct vidtv_mux_pid_ctx 34 *vidtv_mux_get_pid_ctx(struct vidtv_mux *m, u16 pid) 35 { 36 struct vidtv_mux_pid_ctx *ctx; 37 38 hash_for_each_possible(m->pid_ctx, ctx, h, pid) 39 if (ctx->pid == pid) 40 return ctx; 41 return NULL; 42 } 43 44 static struct vidtv_mux_pid_ctx 45 *vidtv_mux_create_pid_ctx_once(struct vidtv_mux *m, u16 pid) 46 { 47 struct vidtv_mux_pid_ctx *ctx; 48 49 ctx = vidtv_mux_get_pid_ctx(m, pid); 50 if (ctx) 51 return ctx; 52 53 ctx = kzalloc(sizeof(*ctx), GFP_KERNEL); 54 if (!ctx) 55 return NULL; 56 57 ctx->pid = pid; 58 ctx->cc = 0; 59 hash_add(m->pid_ctx, &ctx->h, pid); 60 61 return ctx; 62 } 63 64 static void vidtv_mux_pid_ctx_destroy(struct vidtv_mux *m) 65 { 66 struct vidtv_mux_pid_ctx *ctx; 67 struct hlist_node *tmp; 68 int bkt; 69 70 hash_for_each_safe(m->pid_ctx, bkt, tmp, ctx, h) { 71 hash_del(&ctx->h); 72 kfree(ctx); 73 } 74 } 75 76 static int vidtv_mux_pid_ctx_init(struct vidtv_mux *m) 77 { 78 struct vidtv_psi_table_pat_program *p = m->si.pat->program; 79 u16 pid; 80 81 hash_init(m->pid_ctx); 82 /* push the pcr pid ctx */ 83 if (!vidtv_mux_create_pid_ctx_once(m, m->pcr_pid)) 84 return -ENOMEM; 85 /* push the NULL packet pid ctx */ 86 if (!vidtv_mux_create_pid_ctx_once(m, TS_NULL_PACKET_PID)) 87 goto free; 88 /* push the PAT pid ctx */ 89 if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_PAT_PID)) 90 goto free; 91 /* push the SDT pid ctx */ 92 if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_SDT_PID)) 93 goto free; 94 /* push the NIT pid ctx */ 95 if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_NIT_PID)) 96 goto free; 97 /* push the EIT pid ctx */ 98 if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_EIT_PID)) 99 goto free; 100 101 /* add a ctx for all PMT sections */ 102 while (p) { 103 pid = vidtv_psi_get_pat_program_pid(p); 104 vidtv_mux_create_pid_ctx_once(m, pid); 105 p = p->next; 106 } 107 108 return 0; 109 110 free: 111 vidtv_mux_pid_ctx_destroy(m); 112 return -ENOMEM; 113 } 114 115 static void vidtv_mux_update_clk(struct vidtv_mux *m) 116 { 117 /* call this at every thread iteration */ 118 u64 elapsed_time; 119 120 m->timing.past_jiffies = m->timing.current_jiffies; 121 m->timing.current_jiffies = get_jiffies_64(); 122 123 elapsed_time = jiffies_to_usecs(m->timing.current_jiffies - 124 m->timing.past_jiffies); 125 126 /* update the 27Mhz clock proportionally to the elapsed time */ 127 m->timing.clk += (CLOCK_UNIT_27MHZ / USEC_PER_SEC) * elapsed_time; 128 } 129 130 static u32 vidtv_mux_push_si(struct vidtv_mux *m) 131 { 132 struct vidtv_psi_pat_write_args pat_args = { 133 .buf = m->mux_buf, 134 .buf_sz = m->mux_buf_sz, 135 .pat = m->si.pat, 136 }; 137 struct vidtv_psi_pmt_write_args pmt_args = { 138 .buf = m->mux_buf, 139 .buf_sz = m->mux_buf_sz, 140 .pcr_pid = m->pcr_pid, 141 }; 142 struct vidtv_psi_sdt_write_args sdt_args = { 143 .buf = m->mux_buf, 144 .buf_sz = m->mux_buf_sz, 145 .sdt = m->si.sdt, 146 }; 147 struct vidtv_psi_nit_write_args nit_args = { 148 .buf = m->mux_buf, 149 .buf_sz = m->mux_buf_sz, 150 .nit = m->si.nit, 151 152 }; 153 struct vidtv_psi_eit_write_args eit_args = { 154 .buf = m->mux_buf, 155 .buf_sz = m->mux_buf_sz, 156 .eit = m->si.eit, 157 }; 158 u32 initial_offset = m->mux_buf_offset; 159 struct vidtv_mux_pid_ctx *pat_ctx; 160 struct vidtv_mux_pid_ctx *pmt_ctx; 161 struct vidtv_mux_pid_ctx *sdt_ctx; 162 struct vidtv_mux_pid_ctx *nit_ctx; 163 struct vidtv_mux_pid_ctx *eit_ctx; 164 u32 nbytes; 165 u16 pmt_pid; 166 u32 i; 167 168 pat_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_PAT_PID); 169 sdt_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_SDT_PID); 170 nit_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_NIT_PID); 171 eit_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_EIT_PID); 172 173 pat_args.offset = m->mux_buf_offset; 174 pat_args.continuity_counter = &pat_ctx->cc; 175 176 m->mux_buf_offset += vidtv_psi_pat_write_into(&pat_args); 177 178 for (i = 0; i < m->si.pat->num_pmt; ++i) { 179 pmt_pid = vidtv_psi_pmt_get_pid(m->si.pmt_secs[i], 180 m->si.pat); 181 182 if (pmt_pid > TS_LAST_VALID_PID) { 183 dev_warn_ratelimited(m->dev, 184 "PID: %d not found\n", pmt_pid); 185 continue; 186 } 187 188 pmt_ctx = vidtv_mux_get_pid_ctx(m, pmt_pid); 189 190 pmt_args.offset = m->mux_buf_offset; 191 pmt_args.pmt = m->si.pmt_secs[i]; 192 pmt_args.pid = pmt_pid; 193 pmt_args.continuity_counter = &pmt_ctx->cc; 194 195 /* write each section into buffer */ 196 m->mux_buf_offset += vidtv_psi_pmt_write_into(&pmt_args); 197 } 198 199 sdt_args.offset = m->mux_buf_offset; 200 sdt_args.continuity_counter = &sdt_ctx->cc; 201 202 m->mux_buf_offset += vidtv_psi_sdt_write_into(&sdt_args); 203 204 nit_args.offset = m->mux_buf_offset; 205 nit_args.continuity_counter = &nit_ctx->cc; 206 207 m->mux_buf_offset += vidtv_psi_nit_write_into(&nit_args); 208 209 eit_args.offset = m->mux_buf_offset; 210 eit_args.continuity_counter = &eit_ctx->cc; 211 212 m->mux_buf_offset += vidtv_psi_eit_write_into(&eit_args); 213 214 nbytes = m->mux_buf_offset - initial_offset; 215 216 m->num_streamed_si++; 217 218 return nbytes; 219 } 220 221 static u32 vidtv_mux_push_pcr(struct vidtv_mux *m) 222 { 223 struct pcr_write_args args = {}; 224 struct vidtv_mux_pid_ctx *ctx; 225 u32 nbytes = 0; 226 227 ctx = vidtv_mux_get_pid_ctx(m, m->pcr_pid); 228 args.dest_buf = m->mux_buf; 229 args.pid = m->pcr_pid; 230 args.buf_sz = m->mux_buf_sz; 231 args.continuity_counter = &ctx->cc; 232 233 /* the 27Mhz clock will feed both parts of the PCR bitfield */ 234 args.pcr = m->timing.clk; 235 236 nbytes += vidtv_ts_pcr_write_into(args); 237 m->mux_buf_offset += nbytes; 238 239 m->num_streamed_pcr++; 240 241 return nbytes; 242 } 243 244 static bool vidtv_mux_should_push_pcr(struct vidtv_mux *m) 245 { 246 u64 next_pcr_at; 247 248 if (m->num_streamed_pcr == 0) 249 return true; 250 251 next_pcr_at = m->timing.start_jiffies + 252 usecs_to_jiffies(m->num_streamed_pcr * 253 m->timing.pcr_period_usecs); 254 255 return time_after64(m->timing.current_jiffies, next_pcr_at); 256 } 257 258 static bool vidtv_mux_should_push_si(struct vidtv_mux *m) 259 { 260 u64 next_si_at; 261 262 if (m->num_streamed_si == 0) 263 return true; 264 265 next_si_at = m->timing.start_jiffies + 266 usecs_to_jiffies(m->num_streamed_si * 267 m->timing.si_period_usecs); 268 269 return time_after64(m->timing.current_jiffies, next_si_at); 270 } 271 272 static u32 vidtv_mux_packetize_access_units(struct vidtv_mux *m, 273 struct vidtv_encoder *e) 274 { 275 struct pes_write_args args = { 276 .dest_buf = m->mux_buf, 277 .dest_buf_sz = m->mux_buf_sz, 278 .pid = be16_to_cpu(e->es_pid), 279 .encoder_id = e->id, 280 .stream_id = be16_to_cpu(e->stream_id), 281 .send_pts = true, /* forbidden value '01'... */ 282 .send_dts = false, /* ...for PTS_DTS flags */ 283 }; 284 struct vidtv_access_unit *au = e->access_units; 285 u32 initial_offset = m->mux_buf_offset; 286 struct vidtv_mux_pid_ctx *pid_ctx; 287 u32 nbytes = 0; 288 u8 *buf = NULL; 289 290 /* see SMPTE 302M clause 6.4 */ 291 if (args.encoder_id == S302M) { 292 args.send_dts = false; 293 args.send_pts = true; 294 } 295 296 pid_ctx = vidtv_mux_create_pid_ctx_once(m, be16_to_cpu(e->es_pid)); 297 args.continuity_counter = &pid_ctx->cc; 298 299 while (au) { 300 buf = e->encoder_buf + au->offset; 301 args.from = buf; 302 args.access_unit_len = au->nbytes; 303 args.dest_offset = m->mux_buf_offset; 304 args.pts = au->pts; 305 args.pcr = m->timing.clk; 306 307 m->mux_buf_offset += vidtv_pes_write_into(&args); 308 309 au = au->next; 310 } 311 312 /* 313 * clear the encoder state once the ES data has been written to the mux 314 * buffer 315 */ 316 e->clear(e); 317 318 nbytes = m->mux_buf_offset - initial_offset; 319 return nbytes; 320 } 321 322 static u32 vidtv_mux_poll_encoders(struct vidtv_mux *m) 323 { 324 struct vidtv_channel *cur_chnl = m->channels; 325 struct vidtv_encoder *e = NULL; 326 u32 nbytes = 0; 327 u32 au_nbytes; 328 329 while (cur_chnl) { 330 e = cur_chnl->encoders; 331 332 while (e) { 333 e->encode(e); 334 /* get the TS packets into the mux buffer */ 335 au_nbytes = vidtv_mux_packetize_access_units(m, e); 336 nbytes += au_nbytes; 337 m->mux_buf_offset += au_nbytes; 338 /* grab next encoder */ 339 e = e->next; 340 } 341 342 /* grab the next channel */ 343 cur_chnl = cur_chnl->next; 344 } 345 346 return nbytes; 347 } 348 349 static u32 vidtv_mux_pad_with_nulls(struct vidtv_mux *m, u32 npkts) 350 { 351 struct null_packet_write_args args = { 352 .dest_buf = m->mux_buf, 353 .buf_sz = m->mux_buf_sz, 354 .dest_offset = m->mux_buf_offset, 355 }; 356 u32 initial_offset = m->mux_buf_offset; 357 struct vidtv_mux_pid_ctx *ctx; 358 u32 nbytes; 359 u32 i; 360 361 ctx = vidtv_mux_get_pid_ctx(m, TS_NULL_PACKET_PID); 362 363 args.continuity_counter = &ctx->cc; 364 365 for (i = 0; i < npkts; ++i) { 366 m->mux_buf_offset += vidtv_ts_null_write_into(args); 367 args.dest_offset = m->mux_buf_offset; 368 } 369 370 nbytes = m->mux_buf_offset - initial_offset; 371 372 /* sanity check */ 373 if (nbytes != npkts * TS_PACKET_LEN) 374 dev_err_ratelimited(m->dev, "%d != %d\n", 375 nbytes, npkts * TS_PACKET_LEN); 376 377 return nbytes; 378 } 379 380 static void vidtv_mux_clear(struct vidtv_mux *m) 381 { 382 /* clear the packets currently in the mux */ 383 memset(m->mux_buf, 0, m->mux_buf_sz * sizeof(*m->mux_buf)); 384 /* point to the beginning of the buffer again */ 385 m->mux_buf_offset = 0; 386 } 387 388 #define ERR_RATE 10000000 389 static void vidtv_mux_tick(struct work_struct *work) 390 { 391 struct vidtv_mux *m = container_of(work, 392 struct vidtv_mux, 393 mpeg_thread); 394 struct dtv_frontend_properties *c = &m->fe->dtv_property_cache; 395 u32 tot_bits = 0; 396 u32 nbytes; 397 u32 npkts; 398 399 while (m->streaming) { 400 nbytes = 0; 401 402 vidtv_mux_update_clk(m); 403 404 if (vidtv_mux_should_push_pcr(m)) 405 nbytes += vidtv_mux_push_pcr(m); 406 407 if (vidtv_mux_should_push_si(m)) 408 nbytes += vidtv_mux_push_si(m); 409 410 nbytes += vidtv_mux_poll_encoders(m); 411 nbytes += vidtv_mux_pad_with_nulls(m, 256); 412 413 npkts = nbytes / TS_PACKET_LEN; 414 415 /* if the buffer is not aligned there is a bug somewhere */ 416 if (nbytes % TS_PACKET_LEN) 417 dev_err_ratelimited(m->dev, "Misaligned buffer\n"); 418 419 if (m->on_new_packets_available_cb) 420 m->on_new_packets_available_cb(m->priv, 421 m->mux_buf, 422 npkts); 423 424 vidtv_mux_clear(m); 425 426 /* 427 * Update bytes and packet counts at DVBv5 stats 428 * 429 * For now, both pre and post bit counts are identical, 430 * but post BER count can be lower than pre BER, if the error 431 * correction logic discards packages. 432 */ 433 c->pre_bit_count.stat[0].uvalue = nbytes * 8; 434 c->post_bit_count.stat[0].uvalue = nbytes * 8; 435 c->block_count.stat[0].uvalue += npkts; 436 437 /* 438 * Even without any visible errors for the user, the pre-BER 439 * stats usually have an error range up to 1E-6. So, 440 * add some random error increment count to it. 441 * 442 * Please notice that this is a poor guy's implementation, 443 * as it will produce one corrected bit error every time 444 * ceil(total bytes / ERR_RATE) is incremented, without 445 * any sort of (pseudo-)randomness. 446 */ 447 tot_bits += nbytes * 8; 448 if (tot_bits > ERR_RATE) { 449 c->pre_bit_error.stat[0].uvalue++; 450 tot_bits -= ERR_RATE; 451 } 452 453 usleep_range(VIDTV_SLEEP_USECS, VIDTV_MAX_SLEEP_USECS); 454 } 455 } 456 457 void vidtv_mux_start_thread(struct vidtv_mux *m) 458 { 459 if (m->streaming) { 460 dev_warn_ratelimited(m->dev, "Already streaming. Skipping.\n"); 461 return; 462 } 463 464 m->streaming = true; 465 m->timing.start_jiffies = get_jiffies_64(); 466 schedule_work(&m->mpeg_thread); 467 } 468 469 void vidtv_mux_stop_thread(struct vidtv_mux *m) 470 { 471 if (m->streaming) { 472 m->streaming = false; /* thread will quit */ 473 cancel_work_sync(&m->mpeg_thread); 474 } 475 } 476 477 struct vidtv_mux *vidtv_mux_init(struct dvb_frontend *fe, 478 struct device *dev, 479 struct vidtv_mux_init_args *args) 480 { 481 struct vidtv_mux *m; 482 483 m = kzalloc(sizeof(*m), GFP_KERNEL); 484 if (!m) 485 return NULL; 486 487 m->dev = dev; 488 m->fe = fe; 489 m->timing.pcr_period_usecs = args->pcr_period_usecs; 490 m->timing.si_period_usecs = args->si_period_usecs; 491 492 m->mux_rate_kbytes_sec = args->mux_rate_kbytes_sec; 493 494 m->on_new_packets_available_cb = args->on_new_packets_available_cb; 495 496 m->mux_buf = vzalloc(args->mux_buf_sz); 497 if (!m->mux_buf) 498 goto free_mux; 499 500 m->mux_buf_sz = args->mux_buf_sz; 501 502 m->pcr_pid = args->pcr_pid; 503 m->transport_stream_id = args->transport_stream_id; 504 m->priv = args->priv; 505 m->network_id = args->network_id; 506 m->network_name = kstrdup(args->network_name, GFP_KERNEL); 507 if (!m->network_name) 508 goto free_mux_buf; 509 510 m->timing.current_jiffies = get_jiffies_64(); 511 512 if (args->channels) 513 m->channels = args->channels; 514 else 515 if (vidtv_channels_init(m) < 0) 516 goto free_mux_network_name; 517 518 /* will alloc data for pmt_sections after initializing pat */ 519 if (vidtv_channel_si_init(m) < 0) 520 goto free_channels; 521 522 INIT_WORK(&m->mpeg_thread, vidtv_mux_tick); 523 524 if (vidtv_mux_pid_ctx_init(m) < 0) 525 goto free_channel_si; 526 527 return m; 528 529 free_channel_si: 530 vidtv_channel_si_destroy(m); 531 free_channels: 532 vidtv_channels_destroy(m); 533 free_mux_network_name: 534 kfree(m->network_name); 535 free_mux_buf: 536 vfree(m->mux_buf); 537 free_mux: 538 kfree(m); 539 return NULL; 540 } 541 542 void vidtv_mux_destroy(struct vidtv_mux *m) 543 { 544 vidtv_mux_stop_thread(m); 545 vidtv_mux_pid_ctx_destroy(m); 546 vidtv_channel_si_destroy(m); 547 vidtv_channels_destroy(m); 548 kfree(m->network_name); 549 vfree(m->mux_buf); 550 kfree(m); 551 } 552