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(size: 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 | 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(node: &ctx->h); |
72 | kfree(objp: 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, pid: 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(j: 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 | 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(args: &pat_args); |
177 | |
178 | for (i = 0; i < m->si.pat->num_pmt; ++i) { |
179 | pmt_pid = vidtv_psi_pmt_get_pid(section: m->si.pmt_secs[i], |
180 | pat: 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, pid: 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(args: &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(args: &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(args: &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(args: &eit_args); |
213 | |
214 | nbytes = m->mux_buf_offset - initial_offset; |
215 | |
216 | m->num_streamed_si++; |
217 | |
218 | return nbytes; |
219 | } |
220 | |
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, pid: 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 | |
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(u: m->num_streamed_pcr * |
253 | m->timing.pcr_period_usecs); |
254 | |
255 | return time_after64(m->timing.current_jiffies, next_pcr_at); |
256 | } |
257 | |
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(u: m->num_streamed_si * |
267 | m->timing.si_period_usecs); |
268 | |
269 | return time_after64(m->timing.current_jiffies, next_si_at); |
270 | } |
271 | |
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: &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 | |
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 | |
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 | |
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 |
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, npkts: 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 | |
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(work: &m->mpeg_thread); |
467 | } |
468 | |
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(work: &m->mpeg_thread); |
474 | } |
475 | } |
476 | |
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(size: 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(size: 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(s: 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(objp: m->network_name); |
535 | free_mux_buf: |
536 | vfree(addr: m->mux_buf); |
537 | free_mux: |
538 | kfree(objp: m); |
539 | return NULL; |
540 | } |
541 | |
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(objp: m->network_name); |
549 | vfree(addr: m->mux_buf); |
550 | kfree(objp: m); |
551 | } |
552 | |