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