test-drivers/vidtv/vidtv_pes.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Vidtv serves as a reference DVB driver and helps validate the existing APIs
0004  * in the media subsystem. It can also aid developers working on userspace
0005  * applications.
0006  *
0007  * This file contains the logic to translate the ES data for one access unit
0008  * from an encoder into MPEG TS packets. It does so by first encapsulating it
0009  * with a PES header and then splitting it into TS packets.
0010  *
0011  * Copyright (C) 2020 Daniel W. S. Almeida
0012  */
0013
0014 #define pr_fmt(fmt) KBUILD_MODNAME ":%s, %d: " fmt, __func__, __LINE__
0015
0016 #include <linux/types.h>
0017 #include <linux/printk.h>
0018 #include <linux/ratelimit.h>
0019
0020 #include "vidtv_pes.h"
0021 #include "vidtv_common.h"
0022 #include "vidtv_encoder.h"
0023 #include "vidtv_ts.h"
0024
0025 #define PRIVATE_STREAM_1_ID 0xbd /* private_stream_1. See SMPTE 302M-2007 p.6 */
0026 #define PES_HEADER_MAX_STUFFING_BYTES 32
0027 #define PES_TS_HEADER_MAX_STUFFING_BYTES 182
0028
0029 static u32 vidtv_pes_op_get_len(bool send_pts, bool send_dts)
0030 {
0031     u32 len = 0;
0032
0033     /* the flags must always be sent */
0034     len += sizeof(struct vidtv_pes_optional);
0035
0036     /* From all optionals, we might send these for now */
0037     if (send_pts && send_dts)
0038         len += sizeof(struct vidtv_pes_optional_pts_dts);
0039     else if (send_pts)
0040         len += sizeof(struct vidtv_pes_optional_pts);
0041
0042     return len;
0043 }
0044
0045 #define SIZE_PCR (6 + sizeof(struct vidtv_mpeg_ts_adaption))
0046
0047 static u32 vidtv_pes_h_get_len(bool send_pts, bool send_dts)
0048 {
0049     u32 len = 0;
0050
0051     /* PES header length notwithstanding stuffing bytes */
0052
0053     len += sizeof(struct vidtv_mpeg_pes);
0054     len += vidtv_pes_op_get_len(send_pts, send_dts);
0055
0056     return len;
0057 }
0058
0059 static u32 vidtv_pes_write_header_stuffing(struct pes_header_write_args *args)
0060 {
0061     /*
0062      * This is a fixed 8-bit value equal to '0xFF' that can be inserted
0063      * by the encoder, for example to meet the requirements of the channel.
0064      * It is discarded by the decoder. No more than 32 stuffing bytes shall
0065      * be present in one PES packet header.
0066      */
0067     if (args->n_pes_h_s_bytes > PES_HEADER_MAX_STUFFING_BYTES) {
0068         pr_warn_ratelimited("More than %d stuffing bytes in PES packet header\n",
0069                     PES_HEADER_MAX_STUFFING_BYTES);
0070         args->n_pes_h_s_bytes = PES_HEADER_MAX_STUFFING_BYTES;
0071     }
0072
0073     return vidtv_memset(args->dest_buf,
0074                 args->dest_offset,
0075                 args->dest_buf_sz,
0076                 TS_FILL_BYTE,
0077                 args->n_pes_h_s_bytes);
0078 }
0079
0080 static u32 vidtv_pes_write_pts_dts(struct pes_header_write_args *args)
0081 {
0082     u32 nbytes = 0;  /* the number of bytes written by this function */
0083
0084     struct vidtv_pes_optional_pts pts = {};
0085     struct vidtv_pes_optional_pts_dts pts_dts = {};
0086     void *op = NULL;
0087     size_t op_sz = 0;
0088     u64 mask1;
0089     u64 mask2;
0090     u64 mask3;
0091
0092     if (!args->send_pts && args->send_dts)
0093         return 0;
0094
0095     mask1 = GENMASK_ULL(32, 30);
0096     mask2 = GENMASK_ULL(29, 15);
0097     mask3 = GENMASK_ULL(14, 0);
0098
0099     /* see ISO/IEC 13818-1 : 2000 p. 32 */
0100     if (args->send_pts && args->send_dts) {
0101         pts_dts.pts1 = (0x3 << 4) | ((args->pts & mask1) >> 29) | 0x1;
0102         pts_dts.pts2 = cpu_to_be16(((args->pts & mask2) >> 14) | 0x1);
0103         pts_dts.pts3 = cpu_to_be16(((args->pts & mask3) << 1) | 0x1);
0104
0105         pts_dts.dts1 = (0x1 << 4) | ((args->dts & mask1) >> 29) | 0x1;
0106         pts_dts.dts2 = cpu_to_be16(((args->dts & mask2) >> 14) | 0x1);
0107         pts_dts.dts3 = cpu_to_be16(((args->dts & mask3) << 1) | 0x1);
0108
0109         op = &pts_dts;
0110         op_sz = sizeof(pts_dts);
0111
0112     } else if (args->send_pts) {
0113         pts.pts1 = (0x1 << 5) | ((args->pts & mask1) >> 29) | 0x1;
0114         pts.pts2 = cpu_to_be16(((args->pts & mask2) >> 14) | 0x1);
0115         pts.pts3 = cpu_to_be16(((args->pts & mask3) << 1) | 0x1);
0116
0117         op = &pts;
0118         op_sz = sizeof(pts);
0119     }
0120
0121     /* copy PTS/DTS optional */
0122     nbytes += vidtv_memcpy(args->dest_buf,
0123                    args->dest_offset + nbytes,
0124                    args->dest_buf_sz,
0125                    op,
0126                    op_sz);
0127
0128     return nbytes;
0129 }
0130
0131 static u32 vidtv_pes_write_h(struct pes_header_write_args *args)
0132 {
0133     u32 nbytes = 0;  /* the number of bytes written by this function */
0134
0135     struct vidtv_mpeg_pes pes_header          = {};
0136     struct vidtv_pes_optional pes_optional    = {};
0137     struct pes_header_write_args pts_dts_args;
0138     u32 stream_id = (args->encoder_id == S302M) ? PRIVATE_STREAM_1_ID : args->stream_id;
0139     u16 pes_opt_bitfield = 0x01 << 15;
0140
0141     pes_header.bitfield = cpu_to_be32((PES_START_CODE_PREFIX << 8) | stream_id);
0142
0143     pes_header.length = cpu_to_be16(vidtv_pes_op_get_len(args->send_pts,
0144                                  args->send_dts) +
0145                                  args->access_unit_len);
0146
0147     if (args->send_pts && args->send_dts)
0148         pes_opt_bitfield |= (0x3 << 6);
0149     else if (args->send_pts)
0150         pes_opt_bitfield |= (0x1 << 7);
0151
0152     pes_optional.bitfield = cpu_to_be16(pes_opt_bitfield);
0153     pes_optional.length = vidtv_pes_op_get_len(args->send_pts, args->send_dts) +
0154                   args->n_pes_h_s_bytes -
0155                   sizeof(struct vidtv_pes_optional);
0156
0157     /* copy header */
0158     nbytes += vidtv_memcpy(args->dest_buf,
0159                    args->dest_offset + nbytes,
0160                    args->dest_buf_sz,
0161                    &pes_header,
0162                    sizeof(pes_header));
0163
0164     /* copy optional header bits */
0165     nbytes += vidtv_memcpy(args->dest_buf,
0166                    args->dest_offset + nbytes,
0167                    args->dest_buf_sz,
0168                    &pes_optional,
0169                    sizeof(pes_optional));
0170
0171     /* copy the timing information */
0172     pts_dts_args = *args;
0173     pts_dts_args.dest_offset = args->dest_offset + nbytes;
0174     nbytes += vidtv_pes_write_pts_dts(&pts_dts_args);
0175
0176     /* write any PES header stuffing */
0177     nbytes += vidtv_pes_write_header_stuffing(args);
0178
0179     return nbytes;
0180 }
0181
0182 static u32 vidtv_pes_write_pcr_bits(u8 *to, u32 to_offset, u64 pcr)
0183 {
0184     /* Exact same from ffmpeg. PCR is a counter driven by a 27Mhz clock */
0185     u64 div;
0186     u64 rem;
0187     u8 *buf = to + to_offset;
0188     u64 pcr_low;
0189     u64 pcr_high;
0190
0191     div = div64_u64_rem(pcr, 300, &rem);
0192
0193     pcr_low = rem; /* pcr_low = pcr % 300 */
0194     pcr_high = div; /* pcr_high = pcr / 300 */
0195
0196     *buf++ = pcr_high >> 25;
0197     *buf++ = pcr_high >> 17;
0198     *buf++ = pcr_high >>  9;
0199     *buf++ = pcr_high >>  1;
0200     *buf++ = pcr_high <<  7 | pcr_low >> 8 | 0x7e;
0201     *buf++ = pcr_low;
0202
0203     return 6;
0204 }
0205
0206 static u32 vidtv_pes_write_stuffing(struct pes_ts_header_write_args *args,
0207                     u32 dest_offset, bool need_pcr,
0208                     u64 *last_pcr)
0209 {
0210     struct vidtv_mpeg_ts_adaption ts_adap = {};
0211     int stuff_nbytes;
0212     u32 nbytes = 0;
0213
0214     if (!args->n_stuffing_bytes)
0215         return 0;
0216
0217     ts_adap.random_access = 1;
0218
0219     /* length _immediately_ following 'adaptation_field_length' */
0220     if (need_pcr) {
0221         ts_adap.PCR = 1;
0222         ts_adap.length = SIZE_PCR;
0223     } else {
0224         ts_adap.length = sizeof(ts_adap);
0225     }
0226     stuff_nbytes = args->n_stuffing_bytes - ts_adap.length;
0227
0228     ts_adap.length -= sizeof(ts_adap.length);
0229
0230     if (unlikely(stuff_nbytes < 0))
0231         stuff_nbytes = 0;
0232
0233     ts_adap.length += stuff_nbytes;
0234
0235     /* write the adap after the TS header */
0236     nbytes += vidtv_memcpy(args->dest_buf,
0237                    dest_offset + nbytes,
0238                    args->dest_buf_sz,
0239                    &ts_adap,
0240                    sizeof(ts_adap));
0241
0242     /* write the optional PCR */
0243     if (need_pcr) {
0244         nbytes += vidtv_pes_write_pcr_bits(args->dest_buf,
0245                            dest_offset + nbytes,
0246                            args->pcr);
0247
0248         *last_pcr = args->pcr;
0249     }
0250
0251     /* write the stuffing bytes, if are there anything left */
0252     if (stuff_nbytes)
0253         nbytes += vidtv_memset(args->dest_buf,
0254                        dest_offset + nbytes,
0255                        args->dest_buf_sz,
0256                        TS_FILL_BYTE,
0257                        stuff_nbytes);
0258
0259     /*
0260      * The n_stuffing_bytes contain a pre-calculated value of
0261      * the amount of data that this function would read, made from
0262      * vidtv_pes_h_get_len(). If something went wrong, print a warning
0263      */
0264     if (nbytes != args->n_stuffing_bytes)
0265         pr_warn_ratelimited("write size was %d, expected %d\n",
0266                     nbytes, args->n_stuffing_bytes);
0267
0268     return nbytes;
0269 }
0270
0271 static u32 vidtv_pes_write_ts_h(struct pes_ts_header_write_args args,
0272                 bool need_pcr, u64 *last_pcr)
0273 {
0274     /* number of bytes written by this function */
0275     u32 nbytes = 0;
0276     struct vidtv_mpeg_ts ts_header = {};
0277     u16 payload_start = !args.wrote_pes_header;
0278
0279     ts_header.sync_byte        = TS_SYNC_BYTE;
0280     ts_header.bitfield         = cpu_to_be16((payload_start << 14) | args.pid);
0281     ts_header.scrambling       = 0;
0282     ts_header.adaptation_field = (args.n_stuffing_bytes) > 0;
0283     ts_header.payload          = (args.n_stuffing_bytes) < PES_TS_HEADER_MAX_STUFFING_BYTES;
0284
0285     ts_header.continuity_counter = *args.continuity_counter;
0286
0287     vidtv_ts_inc_cc(args.continuity_counter);
0288
0289     /* write the TS header */
0290     nbytes += vidtv_memcpy(args.dest_buf,
0291                    args.dest_offset + nbytes,
0292                    args.dest_buf_sz,
0293                    &ts_header,
0294                    sizeof(ts_header));
0295
0296     /* write stuffing, if any */
0297     nbytes += vidtv_pes_write_stuffing(&args, args.dest_offset + nbytes,
0298                        need_pcr, last_pcr);
0299
0300     return nbytes;
0301 }
0302
0303 u32 vidtv_pes_write_into(struct pes_write_args *args)
0304 {
0305     u32 unaligned_bytes = (args->dest_offset % TS_PACKET_LEN);
0306     struct pes_ts_header_write_args ts_header_args = {
0307         .dest_buf       = args->dest_buf,
0308         .dest_buf_sz        = args->dest_buf_sz,
0309         .pid            = args->pid,
0310         .pcr            = args->pcr,
0311         .continuity_counter = args->continuity_counter,
0312     };
0313     struct pes_header_write_args pes_header_args = {
0314         .dest_buf       = args->dest_buf,
0315         .dest_buf_sz        = args->dest_buf_sz,
0316         .encoder_id     = args->encoder_id,
0317         .send_pts       = args->send_pts,
0318         .pts            = args->pts,
0319         .send_dts       = args->send_dts,
0320         .dts            = args->dts,
0321         .stream_id      = args->stream_id,
0322         .n_pes_h_s_bytes    = args->n_pes_h_s_bytes,
0323         .access_unit_len    = args->access_unit_len,
0324     };
0325     u32 remaining_len = args->access_unit_len;
0326     bool wrote_pes_header = false;
0327     u64 last_pcr = args->pcr;
0328     bool need_pcr = true;
0329     u32 available_space;
0330     u32 payload_size;
0331     u32 stuff_bytes;
0332     u32 nbytes = 0;
0333
0334     if (unaligned_bytes) {
0335         pr_warn_ratelimited("buffer is misaligned, while starting PES\n");
0336
0337         /* forcibly align and hope for the best */
0338         nbytes += vidtv_memset(args->dest_buf,
0339                        args->dest_offset + nbytes,
0340                        args->dest_buf_sz,
0341                        TS_FILL_BYTE,
0342                        TS_PACKET_LEN - unaligned_bytes);
0343     }
0344
0345     while (remaining_len) {
0346         available_space = TS_PAYLOAD_LEN;
0347         /*
0348          * The amount of space initially available in the TS packet.
0349          * if this is the beginning of the PES packet, take into account
0350          * the space needed for the TS header _and_ for the PES header
0351          */
0352         if (!wrote_pes_header)
0353             available_space -= vidtv_pes_h_get_len(args->send_pts,
0354                                    args->send_dts);
0355
0356         /*
0357          * if the encoder has inserted stuffing bytes in the PES
0358          * header, account for them.
0359          */
0360         available_space -= args->n_pes_h_s_bytes;
0361
0362         /* Take the extra adaptation into account if need to send PCR */
0363         if (need_pcr) {
0364             available_space -= SIZE_PCR;
0365             stuff_bytes = SIZE_PCR;
0366         } else {
0367             stuff_bytes = 0;
0368         }
0369
0370         /*
0371          * how much of the _actual_ payload should be written in this
0372          * packet.
0373          */
0374         if (remaining_len >= available_space) {
0375             payload_size = available_space;
0376         } else {
0377             /* Last frame should ensure 188-bytes PS alignment */
0378             payload_size = remaining_len;
0379             stuff_bytes += available_space - payload_size;
0380
0381             /*
0382              * Ensure that the stuff bytes will be within the
0383              * allowed range, decrementing the number of payload
0384              * bytes to write if needed.
0385              */
0386             if (stuff_bytes > PES_TS_HEADER_MAX_STUFFING_BYTES) {
0387                 u32 tmp = stuff_bytes - PES_TS_HEADER_MAX_STUFFING_BYTES;
0388
0389                 stuff_bytes = PES_TS_HEADER_MAX_STUFFING_BYTES;
0390                 payload_size -= tmp;
0391             }
0392         }
0393
0394         /* write ts header */
0395         ts_header_args.dest_offset = args->dest_offset + nbytes;
0396         ts_header_args.wrote_pes_header = wrote_pes_header;
0397         ts_header_args.n_stuffing_bytes = stuff_bytes;
0398
0399         nbytes += vidtv_pes_write_ts_h(ts_header_args, need_pcr,
0400                            &last_pcr);
0401
0402         need_pcr = false;
0403
0404         if (!wrote_pes_header) {
0405             /* write the PES header only once */
0406             pes_header_args.dest_offset = args->dest_offset +
0407                               nbytes;
0408             nbytes += vidtv_pes_write_h(&pes_header_args);
0409             wrote_pes_header = true;
0410         }
0411
0412         /* write as much of the payload as we possibly can */
0413         nbytes += vidtv_memcpy(args->dest_buf,
0414                        args->dest_offset + nbytes,
0415                        args->dest_buf_sz,
0416                        args->from,
0417                        payload_size);
0418
0419         args->from += payload_size;
0420
0421         remaining_len -= payload_size;
0422     }
0423
0424     return nbytes;
0425 }