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(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 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(args->dest_buf, 124 args->dest_offset + nbytes, 125 args->dest_buf_sz, 126 op, 127 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(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 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(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 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(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 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(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 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(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