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 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 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 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 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 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 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 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 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 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