427 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Vidtv serves as a reference DVB driver and helps validate the existing APIs4 * in the media subsystem. It can also aid developers working on userspace5 * applications.6 *7 * This file contains the logic to translate the ES data for one access unit8 * from an encoder into MPEG TS packets. It does so by first encapsulating it9 * with a PES header and then splitting it into TS packets.10 *11 * Copyright (C) 2020 Daniel W. S. Almeida12 */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 3228#define PES_TS_HEADER_MAX_STUFFING_BYTES 18229 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 inserted64 * 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 shall66 * 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 of262 * the amount of data that this function would read, made from263 * vidtv_pes_h_get_len(). If something went wrong, print a warning264 */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 account351 * the space needed for the TS header _and_ for the PES header352 */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 PES359 * 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 this373 * 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 the384 * allowed range, decrementing the number of payload385 * 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