1 files changed, 425 insertions, 0 deletions

diff --git a/drivers/media/test-drivers/vidtv/vidtv_pes.c b/drivers/media/test-drivers/vidtv/vidtv_pes.c
new file mode 100644
index 0000000000..782e5e7fbb
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_pes.c
@@ -0,0 +1,425 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Vidtv serves as a reference DVB driver and helps validate the existing APIs
+ * in the media subsystem. It can also aid developers working on userspace
+ * applications.
+ *
+ * This file contains the logic to translate the ES data for one access unit
+ * from an encoder into MPEG TS packets. It does so by first encapsulating it
+ * with a PES header and then splitting it into TS packets.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ":%s, %d: " fmt, __func__, __LINE__
+
+#include <linux/types.h>
+#include <linux/printk.h>
+#include <linux/ratelimit.h>
+
+#include "vidtv_pes.h"
+#include "vidtv_common.h"
+#include "vidtv_encoder.h"
+#include "vidtv_ts.h"
+
+#define PRIVATE_STREAM_1_ID 0xbd /* private_stream_1. See SMPTE 302M-2007 p.6 */
+#define PES_HEADER_MAX_STUFFING_BYTES 32
+#define PES_TS_HEADER_MAX_STUFFING_BYTES 182
+
+static u32 vidtv_pes_op_get_len(bool send_pts, bool send_dts)
+{
+	u32 len = 0;
+
+	/* the flags must always be sent */
+	len += sizeof(struct vidtv_pes_optional);
+
+	/* From all optionals, we might send these for now */
+	if (send_pts && send_dts)
+		len += sizeof(struct vidtv_pes_optional_pts_dts);
+	else if (send_pts)
+		len += sizeof(struct vidtv_pes_optional_pts);
+
+	return len;
+}
+
+#define SIZE_PCR (6 + sizeof(struct vidtv_mpeg_ts_adaption))
+
+static u32 vidtv_pes_h_get_len(bool send_pts, bool send_dts)
+{
+	u32 len = 0;
+
+	/* PES header length notwithstanding stuffing bytes */
+
+	len += sizeof(struct vidtv_mpeg_pes);
+	len += vidtv_pes_op_get_len(send_pts, send_dts);
+
+	return len;
+}
+
+static u32 vidtv_pes_write_header_stuffing(struct pes_header_write_args *args)
+{
+	/*
+	 * This is a fixed 8-bit value equal to '0xFF' that can be inserted
+	 * by the encoder, for example to meet the requirements of the channel.
+	 * It is discarded by the decoder. No more than 32 stuffing bytes shall
+	 * be present in one PES packet header.
+	 */
+	if (args->n_pes_h_s_bytes > PES_HEADER_MAX_STUFFING_BYTES) {
+		pr_warn_ratelimited("More than %d stuffing bytes in PES packet header\n",
+				    PES_HEADER_MAX_STUFFING_BYTES);
+		args->n_pes_h_s_bytes = PES_HEADER_MAX_STUFFING_BYTES;
+	}
+
+	return vidtv_memset(args->dest_buf,
+			    args->dest_offset,
+			    args->dest_buf_sz,
+			    TS_FILL_BYTE,
+			    args->n_pes_h_s_bytes);
+}
+
+static u32 vidtv_pes_write_pts_dts(struct pes_header_write_args *args)
+{
+	u32 nbytes = 0;  /* the number of bytes written by this function */
+
+	struct vidtv_pes_optional_pts pts = {};
+	struct vidtv_pes_optional_pts_dts pts_dts = {};
+	void *op = NULL;
+	size_t op_sz = 0;
+	u64 mask1;
+	u64 mask2;
+	u64 mask3;
+
+	if (!args->send_pts && args->send_dts)
+		return 0;
+
+	mask1 = GENMASK_ULL(32, 30);
+	mask2 = GENMASK_ULL(29, 15);
+	mask3 = GENMASK_ULL(14, 0);
+
+	/* see ISO/IEC 13818-1 : 2000 p. 32 */
+	if (args->send_pts && args->send_dts) {
+		pts_dts.pts1 = (0x3 << 4) | ((args->pts & mask1) >> 29) | 0x1;
+		pts_dts.pts2 = cpu_to_be16(((args->pts & mask2) >> 14) | 0x1);
+		pts_dts.pts3 = cpu_to_be16(((args->pts & mask3) << 1) | 0x1);
+
+		pts_dts.dts1 = (0x1 << 4) | ((args->dts & mask1) >> 29) | 0x1;
+		pts_dts.dts2 = cpu_to_be16(((args->dts & mask2) >> 14) | 0x1);
+		pts_dts.dts3 = cpu_to_be16(((args->dts & mask3) << 1) | 0x1);
+
+		op = &pts_dts;
+		op_sz = sizeof(pts_dts);
+
+	} else if (args->send_pts) {
+		pts.pts1 = (0x1 << 5) | ((args->pts & mask1) >> 29) | 0x1;
+		pts.pts2 = cpu_to_be16(((args->pts & mask2) >> 14) | 0x1);
+		pts.pts3 = cpu_to_be16(((args->pts & mask3) << 1) | 0x1);
+
+		op = &pts;
+		op_sz = sizeof(pts);
+	}
+
+	/* copy PTS/DTS optional */
+	nbytes += vidtv_memcpy(args->dest_buf,
+			       args->dest_offset + nbytes,
+			       args->dest_buf_sz,
+			       op,
+			       op_sz);
+
+	return nbytes;
+}
+
+static u32 vidtv_pes_write_h(struct pes_header_write_args *args)
+{
+	u32 nbytes = 0;  /* the number of bytes written by this function */
+
+	struct vidtv_mpeg_pes pes_header          = {};
+	struct vidtv_pes_optional pes_optional    = {};
+	struct pes_header_write_args pts_dts_args;
+	u32 stream_id = (args->encoder_id == S302M) ? PRIVATE_STREAM_1_ID : args->stream_id;
+	u16 pes_opt_bitfield = 0x01 << 15;
+
+	pes_header.bitfield = cpu_to_be32((PES_START_CODE_PREFIX << 8) | stream_id);
+
+	pes_header.length = cpu_to_be16(vidtv_pes_op_get_len(args->send_pts,
+							     args->send_dts) +
+							     args->access_unit_len);
+
+	if (args->send_pts && args->send_dts)
+		pes_opt_bitfield |= (0x3 << 6);
+	else if (args->send_pts)
+		pes_opt_bitfield |= (0x1 << 7);
+
+	pes_optional.bitfield = cpu_to_be16(pes_opt_bitfield);
+	pes_optional.length = vidtv_pes_op_get_len(args->send_pts, args->send_dts) +
+			      args->n_pes_h_s_bytes -
+			      sizeof(struct vidtv_pes_optional);
+
+	/* copy header */
+	nbytes += vidtv_memcpy(args->dest_buf,
+			       args->dest_offset + nbytes,
+			       args->dest_buf_sz,
+			       &pes_header,
+			       sizeof(pes_header));
+
+	/* copy optional header bits */
+	nbytes += vidtv_memcpy(args->dest_buf,
+			       args->dest_offset + nbytes,
+			       args->dest_buf_sz,
+			       &pes_optional,
+			       sizeof(pes_optional));
+
+	/* copy the timing information */
+	pts_dts_args = *args;
+	pts_dts_args.dest_offset = args->dest_offset + nbytes;
+	nbytes += vidtv_pes_write_pts_dts(&pts_dts_args);
+
+	/* write any PES header stuffing */
+	nbytes += vidtv_pes_write_header_stuffing(args);
+
+	return nbytes;
+}
+
+static u32 vidtv_pes_write_pcr_bits(u8 *to, u32 to_offset, u64 pcr)
+{
+	/* Exact same from ffmpeg. PCR is a counter driven by a 27Mhz clock */
+	u64 div;
+	u64 rem;
+	u8 *buf = to + to_offset;
+	u64 pcr_low;
+	u64 pcr_high;
+
+	div = div64_u64_rem(pcr, 300, &rem);
+
+	pcr_low = rem; /* pcr_low = pcr % 300 */
+	pcr_high = div; /* pcr_high = pcr / 300 */
+
+	*buf++ = pcr_high >> 25;
+	*buf++ = pcr_high >> 17;
+	*buf++ = pcr_high >>  9;
+	*buf++ = pcr_high >>  1;
+	*buf++ = pcr_high <<  7 | pcr_low >> 8 | 0x7e;
+	*buf++ = pcr_low;
+
+	return 6;
+}
+
+static u32 vidtv_pes_write_stuffing(struct pes_ts_header_write_args *args,
+				    u32 dest_offset, bool need_pcr,
+				    u64 *last_pcr)
+{
+	struct vidtv_mpeg_ts_adaption ts_adap = {};
+	int stuff_nbytes;
+	u32 nbytes = 0;
+
+	if (!args->n_stuffing_bytes)
+		return 0;
+
+	ts_adap.random_access = 1;
+
+	/* length _immediately_ following 'adaptation_field_length' */
+	if (need_pcr) {
+		ts_adap.PCR = 1;
+		ts_adap.length = SIZE_PCR;
+	} else {
+		ts_adap.length = sizeof(ts_adap);
+	}
+	stuff_nbytes = args->n_stuffing_bytes - ts_adap.length;
+
+	ts_adap.length -= sizeof(ts_adap.length);
+
+	if (unlikely(stuff_nbytes < 0))
+		stuff_nbytes = 0;
+
+	ts_adap.length += stuff_nbytes;
+
+	/* write the adap after the TS header */
+	nbytes += vidtv_memcpy(args->dest_buf,
+			       dest_offset + nbytes,
+			       args->dest_buf_sz,
+			       &ts_adap,
+			       sizeof(ts_adap));
+
+	/* write the optional PCR */
+	if (need_pcr) {
+		nbytes += vidtv_pes_write_pcr_bits(args->dest_buf,
+						   dest_offset + nbytes,
+						   args->pcr);
+
+		*last_pcr = args->pcr;
+	}
+
+	/* write the stuffing bytes, if are there anything left */
+	if (stuff_nbytes)
+		nbytes += vidtv_memset(args->dest_buf,
+				       dest_offset + nbytes,
+				       args->dest_buf_sz,
+				       TS_FILL_BYTE,
+				       stuff_nbytes);
+
+	/*
+	 * The n_stuffing_bytes contain a pre-calculated value of
+	 * the amount of data that this function would read, made from
+	 * vidtv_pes_h_get_len(). If something went wrong, print a warning
+	 */
+	if (nbytes != args->n_stuffing_bytes)
+		pr_warn_ratelimited("write size was %d, expected %d\n",
+				    nbytes, args->n_stuffing_bytes);
+
+	return nbytes;
+}
+
+static u32 vidtv_pes_write_ts_h(struct pes_ts_header_write_args args,
+				bool need_pcr, u64 *last_pcr)
+{
+	/* number of bytes written by this function */
+	u32 nbytes = 0;
+	struct vidtv_mpeg_ts ts_header = {};
+	u16 payload_start = !args.wrote_pes_header;
+
+	ts_header.sync_byte        = TS_SYNC_BYTE;
+	ts_header.bitfield         = cpu_to_be16((payload_start << 14) | args.pid);
+	ts_header.scrambling       = 0;
+	ts_header.adaptation_field = (args.n_stuffing_bytes) > 0;
+	ts_header.payload          = (args.n_stuffing_bytes) < PES_TS_HEADER_MAX_STUFFING_BYTES;
+
+	ts_header.continuity_counter = *args.continuity_counter;
+
+	vidtv_ts_inc_cc(args.continuity_counter);
+
+	/* write the TS header */
+	nbytes += vidtv_memcpy(args.dest_buf,
+			       args.dest_offset + nbytes,
+			       args.dest_buf_sz,
+			       &ts_header,
+			       sizeof(ts_header));
+
+	/* write stuffing, if any */
+	nbytes += vidtv_pes_write_stuffing(&args, args.dest_offset + nbytes,
+					   need_pcr, last_pcr);
+
+	return nbytes;
+}
+
+u32 vidtv_pes_write_into(struct pes_write_args *args)
+{
+	u32 unaligned_bytes = (args->dest_offset % TS_PACKET_LEN);
+	struct pes_ts_header_write_args ts_header_args = {
+		.dest_buf		= args->dest_buf,
+		.dest_buf_sz		= args->dest_buf_sz,
+		.pid			= args->pid,
+		.pcr			= args->pcr,
+		.continuity_counter	= args->continuity_counter,
+	};
+	struct pes_header_write_args pes_header_args = {
+		.dest_buf		= args->dest_buf,
+		.dest_buf_sz		= args->dest_buf_sz,
+		.encoder_id		= args->encoder_id,
+		.send_pts		= args->send_pts,
+		.pts			= args->pts,
+		.send_dts		= args->send_dts,
+		.dts			= args->dts,
+		.stream_id		= args->stream_id,
+		.n_pes_h_s_bytes	= args->n_pes_h_s_bytes,
+		.access_unit_len	= args->access_unit_len,
+	};
+	u32 remaining_len = args->access_unit_len;
+	bool wrote_pes_header = false;
+	u64 last_pcr = args->pcr;
+	bool need_pcr = true;
+	u32 available_space;
+	u32 payload_size;
+	u32 stuff_bytes;
+	u32 nbytes = 0;
+
+	if (unaligned_bytes) {
+		pr_warn_ratelimited("buffer is misaligned, while starting PES\n");
+
+		/* forcibly align and hope for the best */
+		nbytes += vidtv_memset(args->dest_buf,
+				       args->dest_offset + nbytes,
+				       args->dest_buf_sz,
+				       TS_FILL_BYTE,
+				       TS_PACKET_LEN - unaligned_bytes);
+	}
+
+	while (remaining_len) {
+		available_space = TS_PAYLOAD_LEN;
+		/*
+		 * The amount of space initially available in the TS packet.
+		 * if this is the beginning of the PES packet, take into account
+		 * the space needed for the TS header _and_ for the PES header
+		 */
+		if (!wrote_pes_header)
+			available_space -= vidtv_pes_h_get_len(args->send_pts,
+							       args->send_dts);
+
+		/*
+		 * if the encoder has inserted stuffing bytes in the PES
+		 * header, account for them.
+		 */
+		available_space -= args->n_pes_h_s_bytes;
+
+		/* Take the extra adaptation into account if need to send PCR */
+		if (need_pcr) {
+			available_space -= SIZE_PCR;
+			stuff_bytes = SIZE_PCR;
+		} else {
+			stuff_bytes = 0;
+		}
+
+		/*
+		 * how much of the _actual_ payload should be written in this
+		 * packet.
+		 */
+		if (remaining_len >= available_space) {
+			payload_size = available_space;
+		} else {
+			/* Last frame should ensure 188-bytes PS alignment */
+			payload_size = remaining_len;
+			stuff_bytes += available_space - payload_size;
+
+			/*
+			 * Ensure that the stuff bytes will be within the
+			 * allowed range, decrementing the number of payload
+			 * bytes to write if needed.
+			 */
+			if (stuff_bytes > PES_TS_HEADER_MAX_STUFFING_BYTES) {
+				u32 tmp = stuff_bytes - PES_TS_HEADER_MAX_STUFFING_BYTES;
+
+				stuff_bytes = PES_TS_HEADER_MAX_STUFFING_BYTES;
+				payload_size -= tmp;
+			}
+		}
+
+		/* write ts header */
+		ts_header_args.dest_offset = args->dest_offset + nbytes;
+		ts_header_args.wrote_pes_header	= wrote_pes_header;
+		ts_header_args.n_stuffing_bytes	= stuff_bytes;
+
+		nbytes += vidtv_pes_write_ts_h(ts_header_args, need_pcr,
+					       &last_pcr);
+
+		need_pcr = false;
+
+		if (!wrote_pes_header) {
+			/* write the PES header only once */
+			pes_header_args.dest_offset = args->dest_offset +
+						      nbytes;
+			nbytes += vidtv_pes_write_h(&pes_header_args);
+			wrote_pes_header = true;
+		}
+
+		/* write as much of the payload as we possibly can */
+		nbytes += vidtv_memcpy(args->dest_buf,
+				       args->dest_offset + nbytes,
+				       args->dest_buf_sz,
+				       args->from,
+				       payload_size);
+
+		args->from += payload_size;
+
+		remaining_len -= payload_size;
+	}
+
+	return nbytes;
+}