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
30static 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
48static 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
60static 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(to: args->dest_buf,
75 to_offset: args->dest_offset,
76 to_size: args->dest_buf_sz,
77 TS_FILL_BYTE,
78 len: args->n_pes_h_s_bytes);
79}
80
81static 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(to: args->dest_buf,
124 to_offset: args->dest_offset + nbytes,
125 to_size: args->dest_buf_sz,
126 from: op,
127 len: op_sz);
128
129 return nbytes;
130}
131
132static 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(send_pts: args->send_pts, send_dts: args->send_dts) +
155 args->n_pes_h_s_bytes -
156 sizeof(struct vidtv_pes_optional);
157
158 /* copy header */
159 nbytes += vidtv_memcpy(to: args->dest_buf,
160 to_offset: args->dest_offset + nbytes,
161 to_size: args->dest_buf_sz,
162 from: &pes_header,
163 len: sizeof(pes_header));
164
165 /* copy optional header bits */
166 nbytes += vidtv_memcpy(to: args->dest_buf,
167 to_offset: args->dest_offset + nbytes,
168 to_size: args->dest_buf_sz,
169 from: &pes_optional,
170 len: 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(args: &pts_dts_args);
176
177 /* write any PES header stuffing */
178 nbytes += vidtv_pes_write_header_stuffing(args);
179
180 return nbytes;
181}
182
183static 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(dividend: pcr, divisor: 300, remainder: &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
207static 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(to: args->dest_buf,
238 to_offset: dest_offset + nbytes,
239 to_size: args->dest_buf_sz,
240 from: &ts_adap,
241 len: sizeof(ts_adap));
242
243 /* write the optional PCR */
244 if (need_pcr) {
245 nbytes += vidtv_pes_write_pcr_bits(to: args->dest_buf,
246 to_offset: dest_offset + nbytes,
247 pcr: 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(to: args->dest_buf,
255 to_offset: dest_offset + nbytes,
256 to_size: args->dest_buf_sz,
257 TS_FILL_BYTE,
258 len: 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
272static 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(continuity_counter: args.continuity_counter);
289
290 /* write the TS header */
291 nbytes += vidtv_memcpy(to: args.dest_buf,
292 to_offset: args.dest_offset + nbytes,
293 to_size: args.dest_buf_sz,
294 from: &ts_header,
295 len: sizeof(ts_header));
296
297 /* write stuffing, if any */
298 nbytes += vidtv_pes_write_stuffing(args: &args, dest_offset: args.dest_offset + nbytes,
299 need_pcr, last_pcr);
300
301 return nbytes;
302}
303
304u32 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(to: args->dest_buf,
340 to_offset: args->dest_offset + nbytes,
341 to_size: 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(send_pts: args->send_pts,
355 send_dts: 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(args: ts_header_args, need_pcr,
401 last_pcr: &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(args: &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(to: args->dest_buf,
415 to_offset: args->dest_offset + nbytes,
416 to_size: args->dest_buf_sz,
417 from: args->from,
418 len: payload_size);
419
420 args->from += payload_size;
421
422 remaining_len -= payload_size;
423 }
424
425 return nbytes;
426}
427

source code of linux/drivers/media/test-drivers/vidtv/vidtv_pes.c