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 	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 
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 	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 
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 
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 
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 
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 
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, 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(&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(&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(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 	m->timing.current_jiffies = get_jiffies_64();
508 
509 	if (args->channels)
510 		m->channels = args->channels;
511 	else
512 		if (vidtv_channels_init(m) < 0)
513 			goto free_mux_buf;
514 
515 	/* will alloc data for pmt_sections after initializing pat */
516 	if (vidtv_channel_si_init(m) < 0)
517 		goto free_channels;
518 
519 	INIT_WORK(&m->mpeg_thread, vidtv_mux_tick);
520 
521 	if (vidtv_mux_pid_ctx_init(m) < 0)
522 		goto free_channel_si;
523 
524 	return m;
525 
526 free_channel_si:
527 	vidtv_channel_si_destroy(m);
528 free_channels:
529 	vidtv_channels_destroy(m);
530 free_mux_buf:
531 	vfree(m->mux_buf);
532 free_mux:
533 	kfree(m);
534 	return NULL;
535 }
536 
537 void vidtv_mux_destroy(struct vidtv_mux *m)
538 {
539 	vidtv_mux_stop_thread(m);
540 	vidtv_mux_pid_ctx_destroy(m);
541 	vidtv_channel_si_destroy(m);
542 	vidtv_channels_destroy(m);
543 	kfree(m->network_name);
544 	vfree(m->mux_buf);
545 	kfree(m);
546 }
547