xref: /linux/drivers/media/test-drivers/vidtv/vidtv_pes.c (revision 06d07429858317ded2db7986113a9e0129cd599b)
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 logic to translate the ES data for one access unit
8  * from an encoder into MPEG TS packets. It does so by first encapsulating it
9  * with a PES header and then splitting it into TS packets.
10  *
11  * Copyright (C) 2020 Daniel W. S. Almeida
12  */
13 
14 #define pr_fmt(fmt) KBUILD_MODNAME ":%s, %d: " fmt, __func__, __LINE__
15 
16 #include <linux/types.h>
17 #include <linux/math64.h>
18 #include <linux/printk.h>
19 #include <linux/ratelimit.h>
20 
21 #include "vidtv_pes.h"
22 #include "vidtv_common.h"
23 #include "vidtv_encoder.h"
24 #include "vidtv_ts.h"
25 
26 #define PRIVATE_STREAM_1_ID 0xbd /* private_stream_1. See SMPTE 302M-2007 p.6 */
27 #define PES_HEADER_MAX_STUFFING_BYTES 32
28 #define PES_TS_HEADER_MAX_STUFFING_BYTES 182
29 
vidtv_pes_op_get_len(bool send_pts,bool send_dts)30 static u32 vidtv_pes_op_get_len(bool send_pts, bool send_dts)
31 {
32 	u32 len = 0;
33 
34 	/* the flags must always be sent */
35 	len += sizeof(struct vidtv_pes_optional);
36 
37 	/* From all optionals, we might send these for now */
38 	if (send_pts && send_dts)
39 		len += sizeof(struct vidtv_pes_optional_pts_dts);
40 	else if (send_pts)
41 		len += sizeof(struct vidtv_pes_optional_pts);
42 
43 	return len;
44 }
45 
46 #define SIZE_PCR (6 + sizeof(struct vidtv_mpeg_ts_adaption))
47 
vidtv_pes_h_get_len(bool send_pts,bool send_dts)48 static u32 vidtv_pes_h_get_len(bool send_pts, bool send_dts)
49 {
50 	u32 len = 0;
51 
52 	/* PES header length notwithstanding stuffing bytes */
53 
54 	len += sizeof(struct vidtv_mpeg_pes);
55 	len += vidtv_pes_op_get_len(send_pts, send_dts);
56 
57 	return len;
58 }
59 
vidtv_pes_write_header_stuffing(struct pes_header_write_args * args)60 static u32 vidtv_pes_write_header_stuffing(struct pes_header_write_args *args)
61 {
62 	/*
63 	 * This is a fixed 8-bit value equal to '0xFF' that can be inserted
64 	 * by the encoder, for example to meet the requirements of the channel.
65 	 * It is discarded by the decoder. No more than 32 stuffing bytes shall
66 	 * be present in one PES packet header.
67 	 */
68 	if (args->n_pes_h_s_bytes > PES_HEADER_MAX_STUFFING_BYTES) {
69 		pr_warn_ratelimited("More than %d stuffing bytes in PES packet header\n",
70 				    PES_HEADER_MAX_STUFFING_BYTES);
71 		args->n_pes_h_s_bytes = PES_HEADER_MAX_STUFFING_BYTES;
72 	}
73 
74 	return vidtv_memset(args->dest_buf,
75 			    args->dest_offset,
76 			    args->dest_buf_sz,
77 			    TS_FILL_BYTE,
78 			    args->n_pes_h_s_bytes);
79 }
80 
vidtv_pes_write_pts_dts(struct pes_header_write_args * args)81 static u32 vidtv_pes_write_pts_dts(struct pes_header_write_args *args)
82 {
83 	u32 nbytes = 0;  /* the number of bytes written by this function */
84 
85 	struct vidtv_pes_optional_pts pts = {};
86 	struct vidtv_pes_optional_pts_dts pts_dts = {};
87 	void *op = NULL;
88 	size_t op_sz = 0;
89 	u64 mask1;
90 	u64 mask2;
91 	u64 mask3;
92 
93 	if (!args->send_pts && args->send_dts)
94 		return 0;
95 
96 	mask1 = GENMASK_ULL(32, 30);
97 	mask2 = GENMASK_ULL(29, 15);
98 	mask3 = GENMASK_ULL(14, 0);
99 
100 	/* see ISO/IEC 13818-1 : 2000 p. 32 */
101 	if (args->send_pts && args->send_dts) {
102 		pts_dts.pts1 = (0x3 << 4) | ((args->pts & mask1) >> 29) | 0x1;
103 		pts_dts.pts2 = cpu_to_be16(((args->pts & mask2) >> 14) | 0x1);
104 		pts_dts.pts3 = cpu_to_be16(((args->pts & mask3) << 1) | 0x1);
105 
106 		pts_dts.dts1 = (0x1 << 4) | ((args->dts & mask1) >> 29) | 0x1;
107 		pts_dts.dts2 = cpu_to_be16(((args->dts & mask2) >> 14) | 0x1);
108 		pts_dts.dts3 = cpu_to_be16(((args->dts & mask3) << 1) | 0x1);
109 
110 		op = &pts_dts;
111 		op_sz = sizeof(pts_dts);
112 
113 	} else if (args->send_pts) {
114 		pts.pts1 = (0x1 << 5) | ((args->pts & mask1) >> 29) | 0x1;
115 		pts.pts2 = cpu_to_be16(((args->pts & mask2) >> 14) | 0x1);
116 		pts.pts3 = cpu_to_be16(((args->pts & mask3) << 1) | 0x1);
117 
118 		op = &pts;
119 		op_sz = sizeof(pts);
120 	}
121 
122 	/* copy PTS/DTS optional */
123 	nbytes += vidtv_memcpy(args->dest_buf,
124 			       args->dest_offset + nbytes,
125 			       args->dest_buf_sz,
126 			       op,
127 			       op_sz);
128 
129 	return nbytes;
130 }
131 
vidtv_pes_write_h(struct pes_header_write_args * args)132 static u32 vidtv_pes_write_h(struct pes_header_write_args *args)
133 {
134 	u32 nbytes = 0;  /* the number of bytes written by this function */
135 
136 	struct vidtv_mpeg_pes pes_header          = {};
137 	struct vidtv_pes_optional pes_optional    = {};
138 	struct pes_header_write_args pts_dts_args;
139 	u32 stream_id = (args->encoder_id == S302M) ? PRIVATE_STREAM_1_ID : args->stream_id;
140 	u16 pes_opt_bitfield = 0x01 << 15;
141 
142 	pes_header.bitfield = cpu_to_be32((PES_START_CODE_PREFIX << 8) | stream_id);
143 
144 	pes_header.length = cpu_to_be16(vidtv_pes_op_get_len(args->send_pts,
145 							     args->send_dts) +
146 							     args->access_unit_len);
147 
148 	if (args->send_pts && args->send_dts)
149 		pes_opt_bitfield |= (0x3 << 6);
150 	else if (args->send_pts)
151 		pes_opt_bitfield |= (0x1 << 7);
152 
153 	pes_optional.bitfield = cpu_to_be16(pes_opt_bitfield);
154 	pes_optional.length = vidtv_pes_op_get_len(args->send_pts, args->send_dts) +
155 			      args->n_pes_h_s_bytes -
156 			      sizeof(struct vidtv_pes_optional);
157 
158 	/* copy header */
159 	nbytes += vidtv_memcpy(args->dest_buf,
160 			       args->dest_offset + nbytes,
161 			       args->dest_buf_sz,
162 			       &pes_header,
163 			       sizeof(pes_header));
164 
165 	/* copy optional header bits */
166 	nbytes += vidtv_memcpy(args->dest_buf,
167 			       args->dest_offset + nbytes,
168 			       args->dest_buf_sz,
169 			       &pes_optional,
170 			       sizeof(pes_optional));
171 
172 	/* copy the timing information */
173 	pts_dts_args = *args;
174 	pts_dts_args.dest_offset = args->dest_offset + nbytes;
175 	nbytes += vidtv_pes_write_pts_dts(&pts_dts_args);
176 
177 	/* write any PES header stuffing */
178 	nbytes += vidtv_pes_write_header_stuffing(args);
179 
180 	return nbytes;
181 }
182 
vidtv_pes_write_pcr_bits(u8 * to,u32 to_offset,u64 pcr)183 static u32 vidtv_pes_write_pcr_bits(u8 *to, u32 to_offset, u64 pcr)
184 {
185 	/* Exact same from ffmpeg. PCR is a counter driven by a 27Mhz clock */
186 	u64 div;
187 	u64 rem;
188 	u8 *buf = to + to_offset;
189 	u64 pcr_low;
190 	u64 pcr_high;
191 
192 	div = div64_u64_rem(pcr, 300, &rem);
193 
194 	pcr_low = rem; /* pcr_low = pcr % 300 */
195 	pcr_high = div; /* pcr_high = pcr / 300 */
196 
197 	*buf++ = pcr_high >> 25;
198 	*buf++ = pcr_high >> 17;
199 	*buf++ = pcr_high >>  9;
200 	*buf++ = pcr_high >>  1;
201 	*buf++ = pcr_high <<  7 | pcr_low >> 8 | 0x7e;
202 	*buf++ = pcr_low;
203 
204 	return 6;
205 }
206 
vidtv_pes_write_stuffing(struct pes_ts_header_write_args * args,u32 dest_offset,bool need_pcr,u64 * last_pcr)207 static u32 vidtv_pes_write_stuffing(struct pes_ts_header_write_args *args,
208 				    u32 dest_offset, bool need_pcr,
209 				    u64 *last_pcr)
210 {
211 	struct vidtv_mpeg_ts_adaption ts_adap = {};
212 	int stuff_nbytes;
213 	u32 nbytes = 0;
214 
215 	if (!args->n_stuffing_bytes)
216 		return 0;
217 
218 	ts_adap.random_access = 1;
219 
220 	/* length _immediately_ following 'adaptation_field_length' */
221 	if (need_pcr) {
222 		ts_adap.PCR = 1;
223 		ts_adap.length = SIZE_PCR;
224 	} else {
225 		ts_adap.length = sizeof(ts_adap);
226 	}
227 	stuff_nbytes = args->n_stuffing_bytes - ts_adap.length;
228 
229 	ts_adap.length -= sizeof(ts_adap.length);
230 
231 	if (unlikely(stuff_nbytes < 0))
232 		stuff_nbytes = 0;
233 
234 	ts_adap.length += stuff_nbytes;
235 
236 	/* write the adap after the TS header */
237 	nbytes += vidtv_memcpy(args->dest_buf,
238 			       dest_offset + nbytes,
239 			       args->dest_buf_sz,
240 			       &ts_adap,
241 			       sizeof(ts_adap));
242 
243 	/* write the optional PCR */
244 	if (need_pcr) {
245 		nbytes += vidtv_pes_write_pcr_bits(args->dest_buf,
246 						   dest_offset + nbytes,
247 						   args->pcr);
248 
249 		*last_pcr = args->pcr;
250 	}
251 
252 	/* write the stuffing bytes, if are there anything left */
253 	if (stuff_nbytes)
254 		nbytes += vidtv_memset(args->dest_buf,
255 				       dest_offset + nbytes,
256 				       args->dest_buf_sz,
257 				       TS_FILL_BYTE,
258 				       stuff_nbytes);
259 
260 	/*
261 	 * The n_stuffing_bytes contain a pre-calculated value of
262 	 * the amount of data that this function would read, made from
263 	 * vidtv_pes_h_get_len(). If something went wrong, print a warning
264 	 */
265 	if (nbytes != args->n_stuffing_bytes)
266 		pr_warn_ratelimited("write size was %d, expected %d\n",
267 				    nbytes, args->n_stuffing_bytes);
268 
269 	return nbytes;
270 }
271 
vidtv_pes_write_ts_h(struct pes_ts_header_write_args args,bool need_pcr,u64 * last_pcr)272 static u32 vidtv_pes_write_ts_h(struct pes_ts_header_write_args args,
273 				bool need_pcr, u64 *last_pcr)
274 {
275 	/* number of bytes written by this function */
276 	u32 nbytes = 0;
277 	struct vidtv_mpeg_ts ts_header = {};
278 	u16 payload_start = !args.wrote_pes_header;
279 
280 	ts_header.sync_byte        = TS_SYNC_BYTE;
281 	ts_header.bitfield         = cpu_to_be16((payload_start << 14) | args.pid);
282 	ts_header.scrambling       = 0;
283 	ts_header.adaptation_field = (args.n_stuffing_bytes) > 0;
284 	ts_header.payload          = (args.n_stuffing_bytes) < PES_TS_HEADER_MAX_STUFFING_BYTES;
285 
286 	ts_header.continuity_counter = *args.continuity_counter;
287 
288 	vidtv_ts_inc_cc(args.continuity_counter);
289 
290 	/* write the TS header */
291 	nbytes += vidtv_memcpy(args.dest_buf,
292 			       args.dest_offset + nbytes,
293 			       args.dest_buf_sz,
294 			       &ts_header,
295 			       sizeof(ts_header));
296 
297 	/* write stuffing, if any */
298 	nbytes += vidtv_pes_write_stuffing(&args, args.dest_offset + nbytes,
299 					   need_pcr, last_pcr);
300 
301 	return nbytes;
302 }
303 
vidtv_pes_write_into(struct pes_write_args * args)304 u32 vidtv_pes_write_into(struct pes_write_args *args)
305 {
306 	u32 unaligned_bytes = (args->dest_offset % TS_PACKET_LEN);
307 	struct pes_ts_header_write_args ts_header_args = {
308 		.dest_buf		= args->dest_buf,
309 		.dest_buf_sz		= args->dest_buf_sz,
310 		.pid			= args->pid,
311 		.pcr			= args->pcr,
312 		.continuity_counter	= args->continuity_counter,
313 	};
314 	struct pes_header_write_args pes_header_args = {
315 		.dest_buf		= args->dest_buf,
316 		.dest_buf_sz		= args->dest_buf_sz,
317 		.encoder_id		= args->encoder_id,
318 		.send_pts		= args->send_pts,
319 		.pts			= args->pts,
320 		.send_dts		= args->send_dts,
321 		.dts			= args->dts,
322 		.stream_id		= args->stream_id,
323 		.n_pes_h_s_bytes	= args->n_pes_h_s_bytes,
324 		.access_unit_len	= args->access_unit_len,
325 	};
326 	u32 remaining_len = args->access_unit_len;
327 	bool wrote_pes_header = false;
328 	u64 last_pcr = args->pcr;
329 	bool need_pcr = true;
330 	u32 available_space;
331 	u32 payload_size;
332 	u32 stuff_bytes;
333 	u32 nbytes = 0;
334 
335 	if (unaligned_bytes) {
336 		pr_warn_ratelimited("buffer is misaligned, while starting PES\n");
337 
338 		/* forcibly align and hope for the best */
339 		nbytes += vidtv_memset(args->dest_buf,
340 				       args->dest_offset + nbytes,
341 				       args->dest_buf_sz,
342 				       TS_FILL_BYTE,
343 				       TS_PACKET_LEN - unaligned_bytes);
344 	}
345 
346 	while (remaining_len) {
347 		available_space = TS_PAYLOAD_LEN;
348 		/*
349 		 * The amount of space initially available in the TS packet.
350 		 * if this is the beginning of the PES packet, take into account
351 		 * the space needed for the TS header _and_ for the PES header
352 		 */
353 		if (!wrote_pes_header)
354 			available_space -= vidtv_pes_h_get_len(args->send_pts,
355 							       args->send_dts);
356 
357 		/*
358 		 * if the encoder has inserted stuffing bytes in the PES
359 		 * header, account for them.
360 		 */
361 		available_space -= args->n_pes_h_s_bytes;
362 
363 		/* Take the extra adaptation into account if need to send PCR */
364 		if (need_pcr) {
365 			available_space -= SIZE_PCR;
366 			stuff_bytes = SIZE_PCR;
367 		} else {
368 			stuff_bytes = 0;
369 		}
370 
371 		/*
372 		 * how much of the _actual_ payload should be written in this
373 		 * packet.
374 		 */
375 		if (remaining_len >= available_space) {
376 			payload_size = available_space;
377 		} else {
378 			/* Last frame should ensure 188-bytes PS alignment */
379 			payload_size = remaining_len;
380 			stuff_bytes += available_space - payload_size;
381 
382 			/*
383 			 * Ensure that the stuff bytes will be within the
384 			 * allowed range, decrementing the number of payload
385 			 * bytes to write if needed.
386 			 */
387 			if (stuff_bytes > PES_TS_HEADER_MAX_STUFFING_BYTES) {
388 				u32 tmp = stuff_bytes - PES_TS_HEADER_MAX_STUFFING_BYTES;
389 
390 				stuff_bytes = PES_TS_HEADER_MAX_STUFFING_BYTES;
391 				payload_size -= tmp;
392 			}
393 		}
394 
395 		/* write ts header */
396 		ts_header_args.dest_offset = args->dest_offset + nbytes;
397 		ts_header_args.wrote_pes_header	= wrote_pes_header;
398 		ts_header_args.n_stuffing_bytes	= stuff_bytes;
399 
400 		nbytes += vidtv_pes_write_ts_h(ts_header_args, need_pcr,
401 					       &last_pcr);
402 
403 		need_pcr = false;
404 
405 		if (!wrote_pes_header) {
406 			/* write the PES header only once */
407 			pes_header_args.dest_offset = args->dest_offset +
408 						      nbytes;
409 			nbytes += vidtv_pes_write_h(&pes_header_args);
410 			wrote_pes_header = true;
411 		}
412 
413 		/* write as much of the payload as we possibly can */
414 		nbytes += vidtv_memcpy(args->dest_buf,
415 				       args->dest_offset + nbytes,
416 				       args->dest_buf_sz,
417 				       args->from,
418 				       payload_size);
419 
420 		args->from += payload_size;
421 
422 		remaining_len -= payload_size;
423 	}
424 
425 	return nbytes;
426 }
427