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