552 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Vidtv serves as a reference DVB driver and helps validate the existing APIs4 * in the media subsystem. It can also aid developers working on userspace5 * applications.6 *7 * This file contains the multiplexer logic for TS packets from different8 * elementary streams9 *10 * Loosely based on libavcodec/mpegtsenc.c11 *12 * Copyright (C) 2020 Daniel W. S. Almeida13 */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 33static struct vidtv_mux_pid_ctx34*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 44static struct vidtv_mux_pid_ctx45*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 64static 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 76static 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 110free:111 vidtv_mux_pid_ctx_destroy(m);112 return -ENOMEM;113}114 115static 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 130static 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 221static 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 244static 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 258static 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 272static 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 mux314 * buffer315 */316 e->clear(e);317 318 nbytes = m->mux_buf_offset - initial_offset;319 return nbytes;320}321 322static 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 349static 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 380static 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 10000000389static 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 stats428 *429 * For now, both pre and post bit counts are identical,430 * but post BER count can be lower than pre BER, if the error431 * 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-BER439 * 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 time444 * ceil(total bytes / ERR_RATE) is incremented, without445 * 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 457void 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 469void 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 477struct 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 else515 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 529free_channel_si:530 vidtv_channel_si_destroy(m);531free_channels:532 vidtv_channels_destroy(m);533free_mux_network_name:534 kfree(m->network_name);535free_mux_buf:536 vfree(m->mux_buf);537free_mux:538 kfree(m);539 return NULL;540}541 542void 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