drivers/media/test-drivers/vidtv/vidtv_mux.c

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