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