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
vidtv_mux_get_pid_ctx(struct vidtv_mux * m,u16 pid)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
vidtv_mux_create_pid_ctx_once(struct vidtv_mux * m,u16 pid)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
vidtv_mux_pid_ctx_destroy(struct vidtv_mux * m)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
vidtv_mux_pid_ctx_init(struct vidtv_mux * m)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
vidtv_mux_update_clk(struct vidtv_mux * m)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
vidtv_mux_push_si(struct vidtv_mux * m)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
vidtv_mux_push_pcr(struct vidtv_mux * m)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
vidtv_mux_should_push_pcr(struct vidtv_mux * m)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
vidtv_mux_should_push_si(struct vidtv_mux * m)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
vidtv_mux_packetize_access_units(struct vidtv_mux * m,struct vidtv_encoder * e)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
vidtv_mux_poll_encoders(struct vidtv_mux * m)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
vidtv_mux_pad_with_nulls(struct vidtv_mux * m,u32 npkts)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
vidtv_mux_clear(struct vidtv_mux * m)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
vidtv_mux_tick(struct work_struct * work)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
vidtv_mux_start_thread(struct vidtv_mux * m)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
vidtv_mux_stop_thread(struct vidtv_mux * m)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
vidtv_mux_init(struct dvb_frontend * fe,struct device * dev,struct vidtv_mux_init_args * args)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
vidtv_mux_destroy(struct vidtv_mux * m)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