Update Linux to v5.10.109
Sourced from [1]
[1] https://cdn.kernel.org/pub/linux/kernel/v5.x/linux-5.10.109.tar.xz
Change-Id: I19bca9fc6762d4e63bcf3e4cba88bbe560d9c76c
Signed-off-by: Olivier Deprez <olivier.deprez@arm.com>
diff --git a/drivers/media/test-drivers/Kconfig b/drivers/media/test-drivers/Kconfig
new file mode 100644
index 0000000..e27d660
--- /dev/null
+++ b/drivers/media/test-drivers/Kconfig
@@ -0,0 +1,42 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+menuconfig V4L_TEST_DRIVERS
+ bool "V4L test drivers"
+ depends on VIDEO_DEV
+
+if V4L_TEST_DRIVERS
+
+source "drivers/media/test-drivers/vimc/Kconfig"
+
+source "drivers/media/test-drivers/vivid/Kconfig"
+
+config VIDEO_VIM2M
+ tristate "Virtual Memory-to-Memory Driver"
+ depends on VIDEO_DEV && VIDEO_V4L2
+ select VIDEOBUF2_VMALLOC
+ select V4L2_MEM2MEM_DEV
+ select MEDIA_CONTROLLER
+ select MEDIA_CONTROLLER_REQUEST_API
+ help
+ This is a virtual test device for the memory-to-memory driver
+ framework.
+
+source "drivers/media/test-drivers/vicodec/Kconfig"
+
+endif #V4L_TEST_DRIVERS
+
+menuconfig DVB_TEST_DRIVERS
+ bool "DVB test drivers"
+ depends on DVB_CORE && MEDIA_SUPPORT && I2C
+ help
+ Enables DVB test drivers.
+
+ This enables the DVB test drivers. They are meant as an aid for
+ DVB device driver writers and developers working on userspace
+ media applications.
+
+if DVB_TEST_DRIVERS
+
+source "drivers/media/test-drivers/vidtv/Kconfig"
+
+endif #DVB_TEST_DRIVERS
diff --git a/drivers/media/test-drivers/Makefile b/drivers/media/test-drivers/Makefile
new file mode 100644
index 0000000..9f0e4eb
--- /dev/null
+++ b/drivers/media/test-drivers/Makefile
@@ -0,0 +1,10 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for the test drivers.
+#
+
+obj-$(CONFIG_VIDEO_VIMC) += vimc/
+obj-$(CONFIG_VIDEO_VIVID) += vivid/
+obj-$(CONFIG_VIDEO_VIM2M) += vim2m.o
+obj-$(CONFIG_VIDEO_VICODEC) += vicodec/
+obj-$(CONFIG_DVB_VIDTV) += vidtv/
diff --git a/drivers/media/test-drivers/vicodec/Kconfig b/drivers/media/test-drivers/vicodec/Kconfig
new file mode 100644
index 0000000..d77c678
--- /dev/null
+++ b/drivers/media/test-drivers/vicodec/Kconfig
@@ -0,0 +1,15 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_VICODEC
+ tristate "Virtual Codec Driver"
+ depends on VIDEO_DEV && VIDEO_V4L2
+ select VIDEOBUF2_VMALLOC
+ select V4L2_MEM2MEM_DEV
+ select MEDIA_CONTROLLER
+ select MEDIA_CONTROLLER_REQUEST_API
+ help
+ Driver for a Virtual Codec
+
+ This driver can be compared to the vim2m driver for emulating
+ a video device node that exposes an emulated hardware codec.
+
+ When in doubt, say N.
diff --git a/drivers/media/test-drivers/vicodec/Makefile b/drivers/media/test-drivers/vicodec/Makefile
new file mode 100644
index 0000000..01bf7e9
--- /dev/null
+++ b/drivers/media/test-drivers/vicodec/Makefile
@@ -0,0 +1,4 @@
+# SPDX-License-Identifier: GPL-2.0
+vicodec-objs := vicodec-core.o codec-fwht.o codec-v4l2-fwht.o
+
+obj-$(CONFIG_VIDEO_VICODEC) += vicodec.o
diff --git a/drivers/media/test-drivers/vicodec/codec-fwht.c b/drivers/media/test-drivers/vicodec/codec-fwht.c
new file mode 100644
index 0000000..31faf31
--- /dev/null
+++ b/drivers/media/test-drivers/vicodec/codec-fwht.c
@@ -0,0 +1,958 @@
+// SPDX-License-Identifier: LGPL-2.1+
+/*
+ * Copyright 2016 Tom aan de Wiel
+ * Copyright 2018 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * 8x8 Fast Walsh Hadamard Transform in sequency order based on the paper:
+ *
+ * A Recursive Algorithm for Sequency-Ordered Fast Walsh Transforms,
+ * R.D. Brown, 1977
+ */
+
+#include <linux/string.h>
+#include <linux/kernel.h>
+#include "codec-fwht.h"
+
+#define OVERFLOW_BIT BIT(14)
+
+/*
+ * Note: bit 0 of the header must always be 0. Otherwise it cannot
+ * be guaranteed that the magic 8 byte sequence (see below) can
+ * never occur in the rlc output.
+ */
+#define PFRAME_BIT BIT(15)
+#define DUPS_MASK 0x1ffe
+
+#define PBLOCK 0
+#define IBLOCK 1
+
+#define ALL_ZEROS 15
+
+static const uint8_t zigzag[64] = {
+ 0,
+ 1, 8,
+ 2, 9, 16,
+ 3, 10, 17, 24,
+ 4, 11, 18, 25, 32,
+ 5, 12, 19, 26, 33, 40,
+ 6, 13, 20, 27, 34, 41, 48,
+ 7, 14, 21, 28, 35, 42, 49, 56,
+ 15, 22, 29, 36, 43, 50, 57,
+ 23, 30, 37, 44, 51, 58,
+ 31, 38, 45, 52, 59,
+ 39, 46, 53, 60,
+ 47, 54, 61,
+ 55, 62,
+ 63,
+};
+
+/*
+ * noinline_for_stack to work around
+ * https://bugs.llvm.org/show_bug.cgi?id=38809
+ */
+static int noinline_for_stack
+rlc(const s16 *in, __be16 *output, int blocktype)
+{
+ s16 block[8 * 8];
+ s16 *wp = block;
+ int i = 0;
+ int x, y;
+ int ret = 0;
+
+ /* read in block from framebuffer */
+ int lastzero_run = 0;
+ int to_encode;
+
+ for (y = 0; y < 8; y++) {
+ for (x = 0; x < 8; x++) {
+ *wp = in[x + y * 8];
+ wp++;
+ }
+ }
+
+ /* keep track of amount of trailing zeros */
+ for (i = 63; i >= 0 && !block[zigzag[i]]; i--)
+ lastzero_run++;
+
+ *output++ = (blocktype == PBLOCK ? htons(PFRAME_BIT) : 0);
+ ret++;
+
+ to_encode = 8 * 8 - (lastzero_run > 14 ? lastzero_run : 0);
+
+ i = 0;
+ while (i < to_encode) {
+ int cnt = 0;
+ int tmp;
+
+ /* count leading zeros */
+ while ((tmp = block[zigzag[i]]) == 0 && cnt < 14) {
+ cnt++;
+ i++;
+ if (i == to_encode) {
+ cnt--;
+ break;
+ }
+ }
+ /* 4 bits for run, 12 for coefficient (quantization by 4) */
+ *output++ = htons((cnt | tmp << 4));
+ i++;
+ ret++;
+ }
+ if (lastzero_run > 14) {
+ *output = htons(ALL_ZEROS | 0);
+ ret++;
+ }
+
+ return ret;
+}
+
+/*
+ * This function will worst-case increase rlc_in by 65*2 bytes:
+ * one s16 value for the header and 8 * 8 coefficients of type s16.
+ */
+static noinline_for_stack u16
+derlc(const __be16 **rlc_in, s16 *dwht_out, const __be16 *end_of_input)
+{
+ /* header */
+ const __be16 *input = *rlc_in;
+ u16 stat;
+ int dec_count = 0;
+ s16 block[8 * 8 + 16];
+ s16 *wp = block;
+ int i;
+
+ if (input > end_of_input)
+ return OVERFLOW_BIT;
+ stat = ntohs(*input++);
+
+ /*
+ * Now de-compress, it expands one byte to up to 15 bytes
+ * (or fills the remainder of the 64 bytes with zeroes if it
+ * is the last byte to expand).
+ *
+ * So block has to be 8 * 8 + 16 bytes, the '+ 16' is to
+ * allow for overflow if the incoming data was malformed.
+ */
+ while (dec_count < 8 * 8) {
+ s16 in;
+ int length;
+ int coeff;
+
+ if (input > end_of_input)
+ return OVERFLOW_BIT;
+ in = ntohs(*input++);
+ length = in & 0xf;
+ coeff = in >> 4;
+
+ /* fill remainder with zeros */
+ if (length == 15) {
+ for (i = 0; i < 64 - dec_count; i++)
+ *wp++ = 0;
+ break;
+ }
+
+ for (i = 0; i < length; i++)
+ *wp++ = 0;
+ *wp++ = coeff;
+ dec_count += length + 1;
+ }
+
+ wp = block;
+
+ for (i = 0; i < 64; i++) {
+ int pos = zigzag[i];
+ int y = pos / 8;
+ int x = pos % 8;
+
+ dwht_out[x + y * 8] = *wp++;
+ }
+ *rlc_in = input;
+ return stat;
+}
+
+static const int quant_table[] = {
+ 2, 2, 2, 2, 2, 2, 2, 2,
+ 2, 2, 2, 2, 2, 2, 2, 2,
+ 2, 2, 2, 2, 2, 2, 2, 3,
+ 2, 2, 2, 2, 2, 2, 3, 6,
+ 2, 2, 2, 2, 2, 3, 6, 6,
+ 2, 2, 2, 2, 3, 6, 6, 6,
+ 2, 2, 2, 3, 6, 6, 6, 6,
+ 2, 2, 3, 6, 6, 6, 6, 8,
+};
+
+static const int quant_table_p[] = {
+ 3, 3, 3, 3, 3, 3, 3, 3,
+ 3, 3, 3, 3, 3, 3, 3, 3,
+ 3, 3, 3, 3, 3, 3, 3, 3,
+ 3, 3, 3, 3, 3, 3, 3, 6,
+ 3, 3, 3, 3, 3, 3, 6, 6,
+ 3, 3, 3, 3, 3, 6, 6, 9,
+ 3, 3, 3, 3, 6, 6, 9, 9,
+ 3, 3, 3, 6, 6, 9, 9, 10,
+};
+
+static void quantize_intra(s16 *coeff, s16 *de_coeff, u16 qp)
+{
+ const int *quant = quant_table;
+ int i, j;
+
+ for (j = 0; j < 8; j++) {
+ for (i = 0; i < 8; i++, quant++, coeff++, de_coeff++) {
+ *coeff >>= *quant;
+ if (*coeff >= -qp && *coeff <= qp)
+ *coeff = *de_coeff = 0;
+ else
+ *de_coeff = *coeff << *quant;
+ }
+ }
+}
+
+static void dequantize_intra(s16 *coeff)
+{
+ const int *quant = quant_table;
+ int i, j;
+
+ for (j = 0; j < 8; j++)
+ for (i = 0; i < 8; i++, quant++, coeff++)
+ *coeff <<= *quant;
+}
+
+static void quantize_inter(s16 *coeff, s16 *de_coeff, u16 qp)
+{
+ const int *quant = quant_table_p;
+ int i, j;
+
+ for (j = 0; j < 8; j++) {
+ for (i = 0; i < 8; i++, quant++, coeff++, de_coeff++) {
+ *coeff >>= *quant;
+ if (*coeff >= -qp && *coeff <= qp)
+ *coeff = *de_coeff = 0;
+ else
+ *de_coeff = *coeff << *quant;
+ }
+ }
+}
+
+static void dequantize_inter(s16 *coeff)
+{
+ const int *quant = quant_table_p;
+ int i, j;
+
+ for (j = 0; j < 8; j++)
+ for (i = 0; i < 8; i++, quant++, coeff++)
+ *coeff <<= *quant;
+}
+
+static void noinline_for_stack fwht(const u8 *block, s16 *output_block,
+ unsigned int stride,
+ unsigned int input_step, bool intra)
+{
+ /* we'll need more than 8 bits for the transformed coefficients */
+ s32 workspace1[8], workspace2[8];
+ const u8 *tmp = block;
+ s16 *out = output_block;
+ int add = intra ? 256 : 0;
+ unsigned int i;
+
+ /* stage 1 */
+ for (i = 0; i < 8; i++, tmp += stride, out += 8) {
+ switch (input_step) {
+ case 1:
+ workspace1[0] = tmp[0] + tmp[1] - add;
+ workspace1[1] = tmp[0] - tmp[1];
+
+ workspace1[2] = tmp[2] + tmp[3] - add;
+ workspace1[3] = tmp[2] - tmp[3];
+
+ workspace1[4] = tmp[4] + tmp[5] - add;
+ workspace1[5] = tmp[4] - tmp[5];
+
+ workspace1[6] = tmp[6] + tmp[7] - add;
+ workspace1[7] = tmp[6] - tmp[7];
+ break;
+ case 2:
+ workspace1[0] = tmp[0] + tmp[2] - add;
+ workspace1[1] = tmp[0] - tmp[2];
+
+ workspace1[2] = tmp[4] + tmp[6] - add;
+ workspace1[3] = tmp[4] - tmp[6];
+
+ workspace1[4] = tmp[8] + tmp[10] - add;
+ workspace1[5] = tmp[8] - tmp[10];
+
+ workspace1[6] = tmp[12] + tmp[14] - add;
+ workspace1[7] = tmp[12] - tmp[14];
+ break;
+ case 3:
+ workspace1[0] = tmp[0] + tmp[3] - add;
+ workspace1[1] = tmp[0] - tmp[3];
+
+ workspace1[2] = tmp[6] + tmp[9] - add;
+ workspace1[3] = tmp[6] - tmp[9];
+
+ workspace1[4] = tmp[12] + tmp[15] - add;
+ workspace1[5] = tmp[12] - tmp[15];
+
+ workspace1[6] = tmp[18] + tmp[21] - add;
+ workspace1[7] = tmp[18] - tmp[21];
+ break;
+ default:
+ workspace1[0] = tmp[0] + tmp[4] - add;
+ workspace1[1] = tmp[0] - tmp[4];
+
+ workspace1[2] = tmp[8] + tmp[12] - add;
+ workspace1[3] = tmp[8] - tmp[12];
+
+ workspace1[4] = tmp[16] + tmp[20] - add;
+ workspace1[5] = tmp[16] - tmp[20];
+
+ workspace1[6] = tmp[24] + tmp[28] - add;
+ workspace1[7] = tmp[24] - tmp[28];
+ break;
+ }
+
+ /* stage 2 */
+ workspace2[0] = workspace1[0] + workspace1[2];
+ workspace2[1] = workspace1[0] - workspace1[2];
+ workspace2[2] = workspace1[1] - workspace1[3];
+ workspace2[3] = workspace1[1] + workspace1[3];
+
+ workspace2[4] = workspace1[4] + workspace1[6];
+ workspace2[5] = workspace1[4] - workspace1[6];
+ workspace2[6] = workspace1[5] - workspace1[7];
+ workspace2[7] = workspace1[5] + workspace1[7];
+
+ /* stage 3 */
+ out[0] = workspace2[0] + workspace2[4];
+ out[1] = workspace2[0] - workspace2[4];
+ out[2] = workspace2[1] - workspace2[5];
+ out[3] = workspace2[1] + workspace2[5];
+ out[4] = workspace2[2] + workspace2[6];
+ out[5] = workspace2[2] - workspace2[6];
+ out[6] = workspace2[3] - workspace2[7];
+ out[7] = workspace2[3] + workspace2[7];
+ }
+
+ out = output_block;
+
+ for (i = 0; i < 8; i++, out++) {
+ /* stage 1 */
+ workspace1[0] = out[0] + out[1 * 8];
+ workspace1[1] = out[0] - out[1 * 8];
+
+ workspace1[2] = out[2 * 8] + out[3 * 8];
+ workspace1[3] = out[2 * 8] - out[3 * 8];
+
+ workspace1[4] = out[4 * 8] + out[5 * 8];
+ workspace1[5] = out[4 * 8] - out[5 * 8];
+
+ workspace1[6] = out[6 * 8] + out[7 * 8];
+ workspace1[7] = out[6 * 8] - out[7 * 8];
+
+ /* stage 2 */
+ workspace2[0] = workspace1[0] + workspace1[2];
+ workspace2[1] = workspace1[0] - workspace1[2];
+ workspace2[2] = workspace1[1] - workspace1[3];
+ workspace2[3] = workspace1[1] + workspace1[3];
+
+ workspace2[4] = workspace1[4] + workspace1[6];
+ workspace2[5] = workspace1[4] - workspace1[6];
+ workspace2[6] = workspace1[5] - workspace1[7];
+ workspace2[7] = workspace1[5] + workspace1[7];
+ /* stage 3 */
+ out[0 * 8] = workspace2[0] + workspace2[4];
+ out[1 * 8] = workspace2[0] - workspace2[4];
+ out[2 * 8] = workspace2[1] - workspace2[5];
+ out[3 * 8] = workspace2[1] + workspace2[5];
+ out[4 * 8] = workspace2[2] + workspace2[6];
+ out[5 * 8] = workspace2[2] - workspace2[6];
+ out[6 * 8] = workspace2[3] - workspace2[7];
+ out[7 * 8] = workspace2[3] + workspace2[7];
+ }
+}
+
+/*
+ * Not the nicest way of doing it, but P-blocks get twice the range of
+ * that of the I-blocks. Therefore we need a type bigger than 8 bits.
+ * Furthermore values can be negative... This is just a version that
+ * works with 16 signed data
+ */
+static void noinline_for_stack
+fwht16(const s16 *block, s16 *output_block, int stride, int intra)
+{
+ /* we'll need more than 8 bits for the transformed coefficients */
+ s32 workspace1[8], workspace2[8];
+ const s16 *tmp = block;
+ s16 *out = output_block;
+ int i;
+
+ for (i = 0; i < 8; i++, tmp += stride, out += 8) {
+ /* stage 1 */
+ workspace1[0] = tmp[0] + tmp[1];
+ workspace1[1] = tmp[0] - tmp[1];
+
+ workspace1[2] = tmp[2] + tmp[3];
+ workspace1[3] = tmp[2] - tmp[3];
+
+ workspace1[4] = tmp[4] + tmp[5];
+ workspace1[5] = tmp[4] - tmp[5];
+
+ workspace1[6] = tmp[6] + tmp[7];
+ workspace1[7] = tmp[6] - tmp[7];
+
+ /* stage 2 */
+ workspace2[0] = workspace1[0] + workspace1[2];
+ workspace2[1] = workspace1[0] - workspace1[2];
+ workspace2[2] = workspace1[1] - workspace1[3];
+ workspace2[3] = workspace1[1] + workspace1[3];
+
+ workspace2[4] = workspace1[4] + workspace1[6];
+ workspace2[5] = workspace1[4] - workspace1[6];
+ workspace2[6] = workspace1[5] - workspace1[7];
+ workspace2[7] = workspace1[5] + workspace1[7];
+
+ /* stage 3 */
+ out[0] = workspace2[0] + workspace2[4];
+ out[1] = workspace2[0] - workspace2[4];
+ out[2] = workspace2[1] - workspace2[5];
+ out[3] = workspace2[1] + workspace2[5];
+ out[4] = workspace2[2] + workspace2[6];
+ out[5] = workspace2[2] - workspace2[6];
+ out[6] = workspace2[3] - workspace2[7];
+ out[7] = workspace2[3] + workspace2[7];
+ }
+
+ out = output_block;
+
+ for (i = 0; i < 8; i++, out++) {
+ /* stage 1 */
+ workspace1[0] = out[0] + out[1*8];
+ workspace1[1] = out[0] - out[1*8];
+
+ workspace1[2] = out[2*8] + out[3*8];
+ workspace1[3] = out[2*8] - out[3*8];
+
+ workspace1[4] = out[4*8] + out[5*8];
+ workspace1[5] = out[4*8] - out[5*8];
+
+ workspace1[6] = out[6*8] + out[7*8];
+ workspace1[7] = out[6*8] - out[7*8];
+
+ /* stage 2 */
+ workspace2[0] = workspace1[0] + workspace1[2];
+ workspace2[1] = workspace1[0] - workspace1[2];
+ workspace2[2] = workspace1[1] - workspace1[3];
+ workspace2[3] = workspace1[1] + workspace1[3];
+
+ workspace2[4] = workspace1[4] + workspace1[6];
+ workspace2[5] = workspace1[4] - workspace1[6];
+ workspace2[6] = workspace1[5] - workspace1[7];
+ workspace2[7] = workspace1[5] + workspace1[7];
+
+ /* stage 3 */
+ out[0*8] = workspace2[0] + workspace2[4];
+ out[1*8] = workspace2[0] - workspace2[4];
+ out[2*8] = workspace2[1] - workspace2[5];
+ out[3*8] = workspace2[1] + workspace2[5];
+ out[4*8] = workspace2[2] + workspace2[6];
+ out[5*8] = workspace2[2] - workspace2[6];
+ out[6*8] = workspace2[3] - workspace2[7];
+ out[7*8] = workspace2[3] + workspace2[7];
+ }
+}
+
+static noinline_for_stack void
+ifwht(const s16 *block, s16 *output_block, int intra)
+{
+ /*
+ * we'll need more than 8 bits for the transformed coefficients
+ * use native unit of cpu
+ */
+ int workspace1[8], workspace2[8];
+ int inter = intra ? 0 : 1;
+ const s16 *tmp = block;
+ s16 *out = output_block;
+ int i;
+
+ for (i = 0; i < 8; i++, tmp += 8, out += 8) {
+ /* stage 1 */
+ workspace1[0] = tmp[0] + tmp[1];
+ workspace1[1] = tmp[0] - tmp[1];
+
+ workspace1[2] = tmp[2] + tmp[3];
+ workspace1[3] = tmp[2] - tmp[3];
+
+ workspace1[4] = tmp[4] + tmp[5];
+ workspace1[5] = tmp[4] - tmp[5];
+
+ workspace1[6] = tmp[6] + tmp[7];
+ workspace1[7] = tmp[6] - tmp[7];
+
+ /* stage 2 */
+ workspace2[0] = workspace1[0] + workspace1[2];
+ workspace2[1] = workspace1[0] - workspace1[2];
+ workspace2[2] = workspace1[1] - workspace1[3];
+ workspace2[3] = workspace1[1] + workspace1[3];
+
+ workspace2[4] = workspace1[4] + workspace1[6];
+ workspace2[5] = workspace1[4] - workspace1[6];
+ workspace2[6] = workspace1[5] - workspace1[7];
+ workspace2[7] = workspace1[5] + workspace1[7];
+
+ /* stage 3 */
+ out[0] = workspace2[0] + workspace2[4];
+ out[1] = workspace2[0] - workspace2[4];
+ out[2] = workspace2[1] - workspace2[5];
+ out[3] = workspace2[1] + workspace2[5];
+ out[4] = workspace2[2] + workspace2[6];
+ out[5] = workspace2[2] - workspace2[6];
+ out[6] = workspace2[3] - workspace2[7];
+ out[7] = workspace2[3] + workspace2[7];
+ }
+
+ out = output_block;
+
+ for (i = 0; i < 8; i++, out++) {
+ /* stage 1 */
+ workspace1[0] = out[0] + out[1 * 8];
+ workspace1[1] = out[0] - out[1 * 8];
+
+ workspace1[2] = out[2 * 8] + out[3 * 8];
+ workspace1[3] = out[2 * 8] - out[3 * 8];
+
+ workspace1[4] = out[4 * 8] + out[5 * 8];
+ workspace1[5] = out[4 * 8] - out[5 * 8];
+
+ workspace1[6] = out[6 * 8] + out[7 * 8];
+ workspace1[7] = out[6 * 8] - out[7 * 8];
+
+ /* stage 2 */
+ workspace2[0] = workspace1[0] + workspace1[2];
+ workspace2[1] = workspace1[0] - workspace1[2];
+ workspace2[2] = workspace1[1] - workspace1[3];
+ workspace2[3] = workspace1[1] + workspace1[3];
+
+ workspace2[4] = workspace1[4] + workspace1[6];
+ workspace2[5] = workspace1[4] - workspace1[6];
+ workspace2[6] = workspace1[5] - workspace1[7];
+ workspace2[7] = workspace1[5] + workspace1[7];
+
+ /* stage 3 */
+ if (inter) {
+ int d;
+
+ out[0 * 8] = workspace2[0] + workspace2[4];
+ out[1 * 8] = workspace2[0] - workspace2[4];
+ out[2 * 8] = workspace2[1] - workspace2[5];
+ out[3 * 8] = workspace2[1] + workspace2[5];
+ out[4 * 8] = workspace2[2] + workspace2[6];
+ out[5 * 8] = workspace2[2] - workspace2[6];
+ out[6 * 8] = workspace2[3] - workspace2[7];
+ out[7 * 8] = workspace2[3] + workspace2[7];
+
+ for (d = 0; d < 8; d++)
+ out[8 * d] >>= 6;
+ } else {
+ int d;
+
+ out[0 * 8] = workspace2[0] + workspace2[4];
+ out[1 * 8] = workspace2[0] - workspace2[4];
+ out[2 * 8] = workspace2[1] - workspace2[5];
+ out[3 * 8] = workspace2[1] + workspace2[5];
+ out[4 * 8] = workspace2[2] + workspace2[6];
+ out[5 * 8] = workspace2[2] - workspace2[6];
+ out[6 * 8] = workspace2[3] - workspace2[7];
+ out[7 * 8] = workspace2[3] + workspace2[7];
+
+ for (d = 0; d < 8; d++) {
+ out[8 * d] >>= 6;
+ out[8 * d] += 128;
+ }
+ }
+ }
+}
+
+static void fill_encoder_block(const u8 *input, s16 *dst,
+ unsigned int stride, unsigned int input_step)
+{
+ int i, j;
+
+ for (i = 0; i < 8; i++) {
+ for (j = 0; j < 8; j++, input += input_step)
+ *dst++ = *input;
+ input += stride - 8 * input_step;
+ }
+}
+
+static int var_intra(const s16 *input)
+{
+ int32_t mean = 0;
+ int32_t ret = 0;
+ const s16 *tmp = input;
+ int i;
+
+ for (i = 0; i < 8 * 8; i++, tmp++)
+ mean += *tmp;
+ mean /= 64;
+ tmp = input;
+ for (i = 0; i < 8 * 8; i++, tmp++)
+ ret += (*tmp - mean) < 0 ? -(*tmp - mean) : (*tmp - mean);
+ return ret;
+}
+
+static int var_inter(const s16 *old, const s16 *new)
+{
+ int32_t ret = 0;
+ int i;
+
+ for (i = 0; i < 8 * 8; i++, old++, new++)
+ ret += (*old - *new) < 0 ? -(*old - *new) : (*old - *new);
+ return ret;
+}
+
+static noinline_for_stack int
+decide_blocktype(const u8 *cur, const u8 *reference, s16 *deltablock,
+ unsigned int stride, unsigned int input_step)
+{
+ s16 tmp[64];
+ s16 old[64];
+ s16 *work = tmp;
+ unsigned int k, l;
+ int vari;
+ int vard;
+
+ fill_encoder_block(cur, tmp, stride, input_step);
+ fill_encoder_block(reference, old, 8, 1);
+ vari = var_intra(tmp);
+
+ for (k = 0; k < 8; k++) {
+ for (l = 0; l < 8; l++) {
+ *deltablock = *work - *reference;
+ deltablock++;
+ work++;
+ reference++;
+ }
+ }
+ deltablock -= 64;
+ vard = var_inter(old, tmp);
+ return vari <= vard ? IBLOCK : PBLOCK;
+}
+
+static void fill_decoder_block(u8 *dst, const s16 *input, int stride,
+ unsigned int dst_step)
+{
+ int i, j;
+
+ for (i = 0; i < 8; i++) {
+ for (j = 0; j < 8; j++, input++, dst += dst_step) {
+ if (*input < 0)
+ *dst = 0;
+ else if (*input > 255)
+ *dst = 255;
+ else
+ *dst = *input;
+ }
+ dst += stride - (8 * dst_step);
+ }
+}
+
+static void add_deltas(s16 *deltas, const u8 *ref, int stride,
+ unsigned int ref_step)
+{
+ int k, l;
+
+ for (k = 0; k < 8; k++) {
+ for (l = 0; l < 8; l++) {
+ *deltas += *ref;
+ ref += ref_step;
+ /*
+ * Due to quantizing, it might possible that the
+ * decoded coefficients are slightly out of range
+ */
+ if (*deltas < 0)
+ *deltas = 0;
+ else if (*deltas > 255)
+ *deltas = 255;
+ deltas++;
+ }
+ ref += stride - (8 * ref_step);
+ }
+}
+
+static u32 encode_plane(u8 *input, u8 *refp, __be16 **rlco, __be16 *rlco_max,
+ struct fwht_cframe *cf, u32 height, u32 width,
+ u32 stride, unsigned int input_step,
+ bool is_intra, bool next_is_intra)
+{
+ u8 *input_start = input;
+ __be16 *rlco_start = *rlco;
+ s16 deltablock[64];
+ __be16 pframe_bit = htons(PFRAME_BIT);
+ u32 encoding = 0;
+ unsigned int last_size = 0;
+ unsigned int i, j;
+
+ width = round_up(width, 8);
+ height = round_up(height, 8);
+
+ for (j = 0; j < height / 8; j++) {
+ input = input_start + j * 8 * stride;
+ for (i = 0; i < width / 8; i++) {
+ /* intra code, first frame is always intra coded. */
+ int blocktype = IBLOCK;
+ unsigned int size;
+
+ if (!is_intra)
+ blocktype = decide_blocktype(input, refp,
+ deltablock, stride, input_step);
+ if (blocktype == IBLOCK) {
+ fwht(input, cf->coeffs, stride, input_step, 1);
+ quantize_intra(cf->coeffs, cf->de_coeffs,
+ cf->i_frame_qp);
+ } else {
+ /* inter code */
+ encoding |= FWHT_FRAME_PCODED;
+ fwht16(deltablock, cf->coeffs, 8, 0);
+ quantize_inter(cf->coeffs, cf->de_coeffs,
+ cf->p_frame_qp);
+ }
+ if (!next_is_intra) {
+ ifwht(cf->de_coeffs, cf->de_fwht, blocktype);
+
+ if (blocktype == PBLOCK)
+ add_deltas(cf->de_fwht, refp, 8, 1);
+ fill_decoder_block(refp, cf->de_fwht, 8, 1);
+ }
+
+ input += 8 * input_step;
+ refp += 8 * 8;
+
+ size = rlc(cf->coeffs, *rlco, blocktype);
+ if (last_size == size &&
+ !memcmp(*rlco + 1, *rlco - size + 1, 2 * size - 2)) {
+ __be16 *last_rlco = *rlco - size;
+ s16 hdr = ntohs(*last_rlco);
+
+ if (!((*last_rlco ^ **rlco) & pframe_bit) &&
+ (hdr & DUPS_MASK) < DUPS_MASK)
+ *last_rlco = htons(hdr + 2);
+ else
+ *rlco += size;
+ } else {
+ *rlco += size;
+ }
+ if (*rlco >= rlco_max) {
+ encoding |= FWHT_FRAME_UNENCODED;
+ goto exit_loop;
+ }
+ last_size = size;
+ }
+ }
+
+exit_loop:
+ if (encoding & FWHT_FRAME_UNENCODED) {
+ u8 *out = (u8 *)rlco_start;
+ u8 *p;
+
+ input = input_start;
+ /*
+ * The compressed stream should never contain the magic
+ * header, so when we copy the YUV data we replace 0xff
+ * by 0xfe. Since YUV is limited range such values
+ * shouldn't appear anyway.
+ */
+ for (j = 0; j < height; j++) {
+ for (i = 0, p = input; i < width; i++, p += input_step)
+ *out++ = (*p == 0xff) ? 0xfe : *p;
+ input += stride;
+ }
+ *rlco = (__be16 *)out;
+ encoding &= ~FWHT_FRAME_PCODED;
+ }
+ return encoding;
+}
+
+u32 fwht_encode_frame(struct fwht_raw_frame *frm,
+ struct fwht_raw_frame *ref_frm,
+ struct fwht_cframe *cf,
+ bool is_intra, bool next_is_intra,
+ unsigned int width, unsigned int height,
+ unsigned int stride, unsigned int chroma_stride)
+{
+ unsigned int size = height * width;
+ __be16 *rlco = cf->rlc_data;
+ __be16 *rlco_max;
+ u32 encoding;
+
+ rlco_max = rlco + size / 2 - 256;
+ encoding = encode_plane(frm->luma, ref_frm->luma, &rlco, rlco_max, cf,
+ height, width, stride,
+ frm->luma_alpha_step, is_intra, next_is_intra);
+ if (encoding & FWHT_FRAME_UNENCODED)
+ encoding |= FWHT_LUMA_UNENCODED;
+ encoding &= ~FWHT_FRAME_UNENCODED;
+
+ if (frm->components_num >= 3) {
+ u32 chroma_h = height / frm->height_div;
+ u32 chroma_w = width / frm->width_div;
+ unsigned int chroma_size = chroma_h * chroma_w;
+
+ rlco_max = rlco + chroma_size / 2 - 256;
+ encoding |= encode_plane(frm->cb, ref_frm->cb, &rlco, rlco_max,
+ cf, chroma_h, chroma_w,
+ chroma_stride, frm->chroma_step,
+ is_intra, next_is_intra);
+ if (encoding & FWHT_FRAME_UNENCODED)
+ encoding |= FWHT_CB_UNENCODED;
+ encoding &= ~FWHT_FRAME_UNENCODED;
+ rlco_max = rlco + chroma_size / 2 - 256;
+ encoding |= encode_plane(frm->cr, ref_frm->cr, &rlco, rlco_max,
+ cf, chroma_h, chroma_w,
+ chroma_stride, frm->chroma_step,
+ is_intra, next_is_intra);
+ if (encoding & FWHT_FRAME_UNENCODED)
+ encoding |= FWHT_CR_UNENCODED;
+ encoding &= ~FWHT_FRAME_UNENCODED;
+ }
+
+ if (frm->components_num == 4) {
+ rlco_max = rlco + size / 2 - 256;
+ encoding |= encode_plane(frm->alpha, ref_frm->alpha, &rlco,
+ rlco_max, cf, height, width,
+ stride, frm->luma_alpha_step,
+ is_intra, next_is_intra);
+ if (encoding & FWHT_FRAME_UNENCODED)
+ encoding |= FWHT_ALPHA_UNENCODED;
+ encoding &= ~FWHT_FRAME_UNENCODED;
+ }
+
+ cf->size = (rlco - cf->rlc_data) * sizeof(*rlco);
+ return encoding;
+}
+
+static bool decode_plane(struct fwht_cframe *cf, const __be16 **rlco,
+ u32 height, u32 width, const u8 *ref, u32 ref_stride,
+ unsigned int ref_step, u8 *dst,
+ unsigned int dst_stride, unsigned int dst_step,
+ bool uncompressed, const __be16 *end_of_rlco_buf)
+{
+ unsigned int copies = 0;
+ s16 copy[8 * 8];
+ u16 stat;
+ unsigned int i, j;
+ bool is_intra = !ref;
+
+ width = round_up(width, 8);
+ height = round_up(height, 8);
+
+ if (uncompressed) {
+ int i;
+
+ if (end_of_rlco_buf + 1 < *rlco + width * height / 2)
+ return false;
+ for (i = 0; i < height; i++) {
+ memcpy(dst, *rlco, width);
+ dst += dst_stride;
+ *rlco += width / 2;
+ }
+ return true;
+ }
+
+ /*
+ * When decoding each macroblock the rlco pointer will be increased
+ * by 65 * 2 bytes worst-case.
+ * To avoid overflow the buffer has to be 65/64th of the actual raw
+ * image size, just in case someone feeds it malicious data.
+ */
+ for (j = 0; j < height / 8; j++) {
+ for (i = 0; i < width / 8; i++) {
+ const u8 *refp = ref + j * 8 * ref_stride +
+ i * 8 * ref_step;
+ u8 *dstp = dst + j * 8 * dst_stride + i * 8 * dst_step;
+
+ if (copies) {
+ memcpy(cf->de_fwht, copy, sizeof(copy));
+ if ((stat & PFRAME_BIT) && !is_intra)
+ add_deltas(cf->de_fwht, refp,
+ ref_stride, ref_step);
+ fill_decoder_block(dstp, cf->de_fwht,
+ dst_stride, dst_step);
+ copies--;
+ continue;
+ }
+
+ stat = derlc(rlco, cf->coeffs, end_of_rlco_buf);
+ if (stat & OVERFLOW_BIT)
+ return false;
+ if ((stat & PFRAME_BIT) && !is_intra)
+ dequantize_inter(cf->coeffs);
+ else
+ dequantize_intra(cf->coeffs);
+
+ ifwht(cf->coeffs, cf->de_fwht,
+ ((stat & PFRAME_BIT) && !is_intra) ? 0 : 1);
+
+ copies = (stat & DUPS_MASK) >> 1;
+ if (copies)
+ memcpy(copy, cf->de_fwht, sizeof(copy));
+ if ((stat & PFRAME_BIT) && !is_intra)
+ add_deltas(cf->de_fwht, refp,
+ ref_stride, ref_step);
+ fill_decoder_block(dstp, cf->de_fwht, dst_stride,
+ dst_step);
+ }
+ }
+ return true;
+}
+
+bool fwht_decode_frame(struct fwht_cframe *cf, u32 hdr_flags,
+ unsigned int components_num, unsigned int width,
+ unsigned int height, const struct fwht_raw_frame *ref,
+ unsigned int ref_stride, unsigned int ref_chroma_stride,
+ struct fwht_raw_frame *dst, unsigned int dst_stride,
+ unsigned int dst_chroma_stride)
+{
+ const __be16 *rlco = cf->rlc_data;
+ const __be16 *end_of_rlco_buf = cf->rlc_data +
+ (cf->size / sizeof(*rlco)) - 1;
+
+ if (!decode_plane(cf, &rlco, height, width, ref->luma, ref_stride,
+ ref->luma_alpha_step, dst->luma, dst_stride,
+ dst->luma_alpha_step,
+ hdr_flags & FWHT_FL_LUMA_IS_UNCOMPRESSED,
+ end_of_rlco_buf))
+ return false;
+
+ if (components_num >= 3) {
+ u32 h = height;
+ u32 w = width;
+
+ if (!(hdr_flags & FWHT_FL_CHROMA_FULL_HEIGHT))
+ h /= 2;
+ if (!(hdr_flags & FWHT_FL_CHROMA_FULL_WIDTH))
+ w /= 2;
+
+ if (!decode_plane(cf, &rlco, h, w, ref->cb, ref_chroma_stride,
+ ref->chroma_step, dst->cb, dst_chroma_stride,
+ dst->chroma_step,
+ hdr_flags & FWHT_FL_CB_IS_UNCOMPRESSED,
+ end_of_rlco_buf))
+ return false;
+ if (!decode_plane(cf, &rlco, h, w, ref->cr, ref_chroma_stride,
+ ref->chroma_step, dst->cr, dst_chroma_stride,
+ dst->chroma_step,
+ hdr_flags & FWHT_FL_CR_IS_UNCOMPRESSED,
+ end_of_rlco_buf))
+ return false;
+ }
+
+ if (components_num == 4)
+ if (!decode_plane(cf, &rlco, height, width, ref->alpha, ref_stride,
+ ref->luma_alpha_step, dst->alpha, dst_stride,
+ dst->luma_alpha_step,
+ hdr_flags & FWHT_FL_ALPHA_IS_UNCOMPRESSED,
+ end_of_rlco_buf))
+ return false;
+ return true;
+}
diff --git a/drivers/media/test-drivers/vicodec/codec-fwht.h b/drivers/media/test-drivers/vicodec/codec-fwht.h
new file mode 100644
index 0000000..b6fec2b
--- /dev/null
+++ b/drivers/media/test-drivers/vicodec/codec-fwht.h
@@ -0,0 +1,150 @@
+/* SPDX-License-Identifier: LGPL-2.1+ */
+/*
+ * Copyright 2016 Tom aan de Wiel
+ * Copyright 2018 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef CODEC_FWHT_H
+#define CODEC_FWHT_H
+
+#include <linux/types.h>
+#include <linux/bitops.h>
+#include <asm/byteorder.h>
+
+/*
+ * The compressed format consists of a fwht_cframe_hdr struct followed by the
+ * compressed frame data. The header contains the size of that data.
+ * Each Y, Cb and Cr plane is compressed separately. If the compressed
+ * size of each plane becomes larger than the uncompressed size, then
+ * that plane is stored uncompressed and the corresponding bit is set
+ * in the flags field of the header.
+ *
+ * Each compressed plane consists of macroblocks and each macroblock
+ * is run-length-encoded. Each macroblock starts with a 16 bit value.
+ * Bit 15 indicates if this is a P-coded macroblock (1) or not (0).
+ * P-coded macroblocks contain a delta against the previous frame.
+ *
+ * Bits 1-12 contain a number. If non-zero, then this same macroblock
+ * repeats that number of times. This results in a high degree of
+ * compression for generated images like colorbars.
+ *
+ * Following this macroblock header the MB coefficients are run-length
+ * encoded: the top 12 bits contain the coefficient, the bottom 4 bits
+ * tell how many times this coefficient occurs. The value 0xf indicates
+ * that the remainder of the macroblock should be filled with zeroes.
+ *
+ * All 16 and 32 bit values are stored in big-endian (network) order.
+ *
+ * Each fwht_cframe_hdr starts with an 8 byte magic header that is
+ * guaranteed not to occur in the compressed frame data. This header
+ * can be used to sync to the next frame.
+ *
+ * This codec uses the Fast Walsh Hadamard Transform. Tom aan de Wiel
+ * developed this as part of a university project, specifically for use
+ * with this driver. His project report can be found here:
+ *
+ * https://hverkuil.home.xs4all.nl/fwht.pdf
+ */
+
+/*
+ * This is a sequence of 8 bytes with the low 4 bits set to 0xf.
+ *
+ * This sequence cannot occur in the encoded data
+ *
+ * Note that these two magic values are symmetrical so endian issues here.
+ */
+#define FWHT_MAGIC1 0x4f4f4f4f
+#define FWHT_MAGIC2 0xffffffff
+
+#define FWHT_VERSION 3
+
+/* Set if this is an interlaced format */
+#define FWHT_FL_IS_INTERLACED BIT(0)
+/* Set if this is a bottom-first (NTSC) interlaced format */
+#define FWHT_FL_IS_BOTTOM_FIRST BIT(1)
+/* Set if each 'frame' contains just one field */
+#define FWHT_FL_IS_ALTERNATE BIT(2)
+/*
+ * If FWHT_FL_IS_ALTERNATE was set, then this is set if this
+ * 'frame' is the bottom field, else it is the top field.
+ */
+#define FWHT_FL_IS_BOTTOM_FIELD BIT(3)
+/* Set if this frame is uncompressed */
+#define FWHT_FL_LUMA_IS_UNCOMPRESSED BIT(4)
+#define FWHT_FL_CB_IS_UNCOMPRESSED BIT(5)
+#define FWHT_FL_CR_IS_UNCOMPRESSED BIT(6)
+#define FWHT_FL_CHROMA_FULL_HEIGHT BIT(7)
+#define FWHT_FL_CHROMA_FULL_WIDTH BIT(8)
+#define FWHT_FL_ALPHA_IS_UNCOMPRESSED BIT(9)
+#define FWHT_FL_I_FRAME BIT(10)
+
+/* A 4-values flag - the number of components - 1 */
+#define FWHT_FL_COMPONENTS_NUM_MSK GENMASK(18, 16)
+#define FWHT_FL_COMPONENTS_NUM_OFFSET 16
+
+#define FWHT_FL_PIXENC_MSK GENMASK(20, 19)
+#define FWHT_FL_PIXENC_OFFSET 19
+#define FWHT_FL_PIXENC_YUV (1 << FWHT_FL_PIXENC_OFFSET)
+#define FWHT_FL_PIXENC_RGB (2 << FWHT_FL_PIXENC_OFFSET)
+#define FWHT_FL_PIXENC_HSV (3 << FWHT_FL_PIXENC_OFFSET)
+
+/*
+ * A macro to calculate the needed padding in order to make sure
+ * both luma and chroma components resolutions are rounded up to
+ * a multiple of 8
+ */
+#define vic_round_dim(dim, div) (round_up((dim) / (div), 8) * (div))
+
+struct fwht_cframe_hdr {
+ u32 magic1;
+ u32 magic2;
+ __be32 version;
+ __be32 width, height;
+ __be32 flags;
+ __be32 colorspace;
+ __be32 xfer_func;
+ __be32 ycbcr_enc;
+ __be32 quantization;
+ __be32 size;
+};
+
+struct fwht_cframe {
+ u16 i_frame_qp;
+ u16 p_frame_qp;
+ __be16 *rlc_data;
+ s16 coeffs[8 * 8];
+ s16 de_coeffs[8 * 8];
+ s16 de_fwht[8 * 8];
+ u32 size;
+};
+
+struct fwht_raw_frame {
+ unsigned int width_div;
+ unsigned int height_div;
+ unsigned int luma_alpha_step;
+ unsigned int chroma_step;
+ unsigned int components_num;
+ u8 *buf;
+ u8 *luma, *cb, *cr, *alpha;
+};
+
+#define FWHT_FRAME_PCODED BIT(0)
+#define FWHT_FRAME_UNENCODED BIT(1)
+#define FWHT_LUMA_UNENCODED BIT(2)
+#define FWHT_CB_UNENCODED BIT(3)
+#define FWHT_CR_UNENCODED BIT(4)
+#define FWHT_ALPHA_UNENCODED BIT(5)
+
+u32 fwht_encode_frame(struct fwht_raw_frame *frm,
+ struct fwht_raw_frame *ref_frm,
+ struct fwht_cframe *cf,
+ bool is_intra, bool next_is_intra,
+ unsigned int width, unsigned int height,
+ unsigned int stride, unsigned int chroma_stride);
+bool fwht_decode_frame(struct fwht_cframe *cf, u32 hdr_flags,
+ unsigned int components_num, unsigned int width,
+ unsigned int height, const struct fwht_raw_frame *ref,
+ unsigned int ref_stride, unsigned int ref_chroma_stride,
+ struct fwht_raw_frame *dst, unsigned int dst_stride,
+ unsigned int dst_chroma_stride);
+#endif
diff --git a/drivers/media/test-drivers/vicodec/codec-v4l2-fwht.c b/drivers/media/test-drivers/vicodec/codec-v4l2-fwht.c
new file mode 100644
index 0000000..b6e39fb
--- /dev/null
+++ b/drivers/media/test-drivers/vicodec/codec-v4l2-fwht.c
@@ -0,0 +1,367 @@
+// SPDX-License-Identifier: LGPL-2.1
+/*
+ * A V4L2 frontend for the FWHT codec
+ *
+ * Copyright 2018 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/videodev2.h>
+#include "codec-v4l2-fwht.h"
+
+static const struct v4l2_fwht_pixfmt_info v4l2_fwht_pixfmts[] = {
+ { V4L2_PIX_FMT_YUV420, 1, 3, 2, 1, 1, 2, 2, 3, 3, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_YVU420, 1, 3, 2, 1, 1, 2, 2, 3, 3, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_YUV422P, 1, 2, 1, 1, 1, 2, 1, 3, 3, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_NV12, 1, 3, 2, 1, 2, 2, 2, 3, 2, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_NV21, 1, 3, 2, 1, 2, 2, 2, 3, 2, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_NV16, 1, 2, 1, 1, 2, 2, 1, 3, 2, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_NV61, 1, 2, 1, 1, 2, 2, 1, 3, 2, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_NV24, 1, 3, 1, 1, 2, 1, 1, 3, 2, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_NV42, 1, 3, 1, 1, 2, 1, 1, 3, 2, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_YUYV, 2, 2, 1, 2, 4, 2, 1, 3, 1, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_YVYU, 2, 2, 1, 2, 4, 2, 1, 3, 1, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_UYVY, 2, 2, 1, 2, 4, 2, 1, 3, 1, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_VYUY, 2, 2, 1, 2, 4, 2, 1, 3, 1, FWHT_FL_PIXENC_YUV},
+ { V4L2_PIX_FMT_BGR24, 3, 3, 1, 3, 3, 1, 1, 3, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_RGB24, 3, 3, 1, 3, 3, 1, 1, 3, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_HSV24, 3, 3, 1, 3, 3, 1, 1, 3, 1, FWHT_FL_PIXENC_HSV},
+ { V4L2_PIX_FMT_BGR32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_XBGR32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_ABGR32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_RGB32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_XRGB32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_ARGB32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_BGRX32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_BGRA32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_RGBX32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_RGBA32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_RGB},
+ { V4L2_PIX_FMT_HSV32, 4, 4, 1, 4, 4, 1, 1, 4, 1, FWHT_FL_PIXENC_HSV},
+ { V4L2_PIX_FMT_GREY, 1, 1, 1, 1, 0, 1, 1, 1, 1, FWHT_FL_PIXENC_RGB},
+};
+
+bool v4l2_fwht_validate_fmt(const struct v4l2_fwht_pixfmt_info *info,
+ u32 width_div, u32 height_div, u32 components_num,
+ u32 pixenc)
+{
+ if (info->width_div == width_div &&
+ info->height_div == height_div &&
+ (!pixenc || info->pixenc == pixenc) &&
+ info->components_num == components_num)
+ return true;
+ return false;
+}
+
+const struct v4l2_fwht_pixfmt_info *v4l2_fwht_find_nth_fmt(u32 width_div,
+ u32 height_div,
+ u32 components_num,
+ u32 pixenc,
+ unsigned int start_idx)
+{
+ unsigned int i;
+
+ for (i = 0; i < ARRAY_SIZE(v4l2_fwht_pixfmts); i++) {
+ bool is_valid = v4l2_fwht_validate_fmt(&v4l2_fwht_pixfmts[i],
+ width_div, height_div,
+ components_num, pixenc);
+ if (is_valid) {
+ if (start_idx == 0)
+ return v4l2_fwht_pixfmts + i;
+ start_idx--;
+ }
+ }
+ return NULL;
+}
+
+const struct v4l2_fwht_pixfmt_info *v4l2_fwht_find_pixfmt(u32 pixelformat)
+{
+ unsigned int i;
+
+ for (i = 0; i < ARRAY_SIZE(v4l2_fwht_pixfmts); i++)
+ if (v4l2_fwht_pixfmts[i].id == pixelformat)
+ return v4l2_fwht_pixfmts + i;
+ return NULL;
+}
+
+const struct v4l2_fwht_pixfmt_info *v4l2_fwht_get_pixfmt(u32 idx)
+{
+ if (idx >= ARRAY_SIZE(v4l2_fwht_pixfmts))
+ return NULL;
+ return v4l2_fwht_pixfmts + idx;
+}
+
+static int prepare_raw_frame(struct fwht_raw_frame *rf,
+ const struct v4l2_fwht_pixfmt_info *info, u8 *buf,
+ unsigned int size)
+{
+ rf->luma = buf;
+ rf->width_div = info->width_div;
+ rf->height_div = info->height_div;
+ rf->luma_alpha_step = info->luma_alpha_step;
+ rf->chroma_step = info->chroma_step;
+ rf->alpha = NULL;
+ rf->components_num = info->components_num;
+
+ /*
+ * The buffer is NULL if it is the reference
+ * frame of an I-frame in the stateless decoder
+ */
+ if (!buf) {
+ rf->luma = NULL;
+ rf->cb = NULL;
+ rf->cr = NULL;
+ rf->alpha = NULL;
+ return 0;
+ }
+ switch (info->id) {
+ case V4L2_PIX_FMT_GREY:
+ rf->cb = NULL;
+ rf->cr = NULL;
+ break;
+ case V4L2_PIX_FMT_YUV420:
+ rf->cb = rf->luma + size;
+ rf->cr = rf->cb + size / 4;
+ break;
+ case V4L2_PIX_FMT_YVU420:
+ rf->cr = rf->luma + size;
+ rf->cb = rf->cr + size / 4;
+ break;
+ case V4L2_PIX_FMT_YUV422P:
+ rf->cb = rf->luma + size;
+ rf->cr = rf->cb + size / 2;
+ break;
+ case V4L2_PIX_FMT_NV12:
+ case V4L2_PIX_FMT_NV16:
+ case V4L2_PIX_FMT_NV24:
+ rf->cb = rf->luma + size;
+ rf->cr = rf->cb + 1;
+ break;
+ case V4L2_PIX_FMT_NV21:
+ case V4L2_PIX_FMT_NV61:
+ case V4L2_PIX_FMT_NV42:
+ rf->cr = rf->luma + size;
+ rf->cb = rf->cr + 1;
+ break;
+ case V4L2_PIX_FMT_YUYV:
+ rf->cb = rf->luma + 1;
+ rf->cr = rf->cb + 2;
+ break;
+ case V4L2_PIX_FMT_YVYU:
+ rf->cr = rf->luma + 1;
+ rf->cb = rf->cr + 2;
+ break;
+ case V4L2_PIX_FMT_UYVY:
+ rf->cb = rf->luma;
+ rf->cr = rf->cb + 2;
+ rf->luma++;
+ break;
+ case V4L2_PIX_FMT_VYUY:
+ rf->cr = rf->luma;
+ rf->cb = rf->cr + 2;
+ rf->luma++;
+ break;
+ case V4L2_PIX_FMT_RGB24:
+ case V4L2_PIX_FMT_HSV24:
+ rf->cr = rf->luma;
+ rf->cb = rf->cr + 2;
+ rf->luma++;
+ break;
+ case V4L2_PIX_FMT_BGR24:
+ rf->cb = rf->luma;
+ rf->cr = rf->cb + 2;
+ rf->luma++;
+ break;
+ case V4L2_PIX_FMT_RGB32:
+ case V4L2_PIX_FMT_XRGB32:
+ case V4L2_PIX_FMT_HSV32:
+ case V4L2_PIX_FMT_ARGB32:
+ rf->alpha = rf->luma;
+ rf->cr = rf->luma + 1;
+ rf->cb = rf->cr + 2;
+ rf->luma += 2;
+ break;
+ case V4L2_PIX_FMT_BGR32:
+ case V4L2_PIX_FMT_XBGR32:
+ case V4L2_PIX_FMT_ABGR32:
+ rf->cb = rf->luma;
+ rf->cr = rf->cb + 2;
+ rf->luma++;
+ rf->alpha = rf->cr + 1;
+ break;
+ case V4L2_PIX_FMT_BGRX32:
+ case V4L2_PIX_FMT_BGRA32:
+ rf->alpha = rf->luma;
+ rf->cb = rf->luma + 1;
+ rf->cr = rf->cb + 2;
+ rf->luma += 2;
+ break;
+ case V4L2_PIX_FMT_RGBX32:
+ case V4L2_PIX_FMT_RGBA32:
+ rf->alpha = rf->luma + 3;
+ rf->cr = rf->luma;
+ rf->cb = rf->cr + 2;
+ rf->luma++;
+ break;
+ default:
+ return -EINVAL;
+ }
+ return 0;
+}
+
+int v4l2_fwht_encode(struct v4l2_fwht_state *state, u8 *p_in, u8 *p_out)
+{
+ unsigned int size = state->stride * state->coded_height;
+ unsigned int chroma_stride = state->stride;
+ const struct v4l2_fwht_pixfmt_info *info = state->info;
+ struct fwht_cframe_hdr *p_hdr;
+ struct fwht_cframe cf;
+ struct fwht_raw_frame rf;
+ u32 encoding;
+ u32 flags = 0;
+
+ if (!info)
+ return -EINVAL;
+
+ if (prepare_raw_frame(&rf, info, p_in, size))
+ return -EINVAL;
+
+ if (info->planes_num == 3)
+ chroma_stride /= 2;
+
+ if (info->id == V4L2_PIX_FMT_NV24 ||
+ info->id == V4L2_PIX_FMT_NV42)
+ chroma_stride *= 2;
+
+ cf.i_frame_qp = state->i_frame_qp;
+ cf.p_frame_qp = state->p_frame_qp;
+ cf.rlc_data = (__be16 *)(p_out + sizeof(*p_hdr));
+
+ encoding = fwht_encode_frame(&rf, &state->ref_frame, &cf,
+ !state->gop_cnt,
+ state->gop_cnt == state->gop_size - 1,
+ state->visible_width,
+ state->visible_height,
+ state->stride, chroma_stride);
+ if (!(encoding & FWHT_FRAME_PCODED))
+ state->gop_cnt = 0;
+ if (++state->gop_cnt >= state->gop_size)
+ state->gop_cnt = 0;
+
+ p_hdr = (struct fwht_cframe_hdr *)p_out;
+ p_hdr->magic1 = FWHT_MAGIC1;
+ p_hdr->magic2 = FWHT_MAGIC2;
+ p_hdr->version = htonl(FWHT_VERSION);
+ p_hdr->width = htonl(state->visible_width);
+ p_hdr->height = htonl(state->visible_height);
+ flags |= (info->components_num - 1) << FWHT_FL_COMPONENTS_NUM_OFFSET;
+ flags |= info->pixenc;
+ if (encoding & FWHT_LUMA_UNENCODED)
+ flags |= FWHT_FL_LUMA_IS_UNCOMPRESSED;
+ if (encoding & FWHT_CB_UNENCODED)
+ flags |= FWHT_FL_CB_IS_UNCOMPRESSED;
+ if (encoding & FWHT_CR_UNENCODED)
+ flags |= FWHT_FL_CR_IS_UNCOMPRESSED;
+ if (encoding & FWHT_ALPHA_UNENCODED)
+ flags |= FWHT_FL_ALPHA_IS_UNCOMPRESSED;
+ if (!(encoding & FWHT_FRAME_PCODED))
+ flags |= FWHT_FL_I_FRAME;
+ if (rf.height_div == 1)
+ flags |= FWHT_FL_CHROMA_FULL_HEIGHT;
+ if (rf.width_div == 1)
+ flags |= FWHT_FL_CHROMA_FULL_WIDTH;
+ p_hdr->flags = htonl(flags);
+ p_hdr->colorspace = htonl(state->colorspace);
+ p_hdr->xfer_func = htonl(state->xfer_func);
+ p_hdr->ycbcr_enc = htonl(state->ycbcr_enc);
+ p_hdr->quantization = htonl(state->quantization);
+ p_hdr->size = htonl(cf.size);
+ return cf.size + sizeof(*p_hdr);
+}
+
+int v4l2_fwht_decode(struct v4l2_fwht_state *state, u8 *p_in, u8 *p_out)
+{
+ u32 flags;
+ struct fwht_cframe cf;
+ unsigned int components_num = 3;
+ unsigned int version;
+ const struct v4l2_fwht_pixfmt_info *info;
+ unsigned int hdr_width_div, hdr_height_div;
+ struct fwht_raw_frame dst_rf;
+ unsigned int dst_chroma_stride = state->stride;
+ unsigned int ref_chroma_stride = state->ref_stride;
+ unsigned int dst_size = state->stride * state->coded_height;
+ unsigned int ref_size;
+
+ if (!state->info)
+ return -EINVAL;
+
+ info = state->info;
+
+ version = ntohl(state->header.version);
+ if (!version || version > FWHT_VERSION) {
+ pr_err("version %d is not supported, current version is %d\n",
+ version, FWHT_VERSION);
+ return -EINVAL;
+ }
+
+ if (state->header.magic1 != FWHT_MAGIC1 ||
+ state->header.magic2 != FWHT_MAGIC2)
+ return -EINVAL;
+
+ /* TODO: support resolution changes */
+ if (ntohl(state->header.width) != state->visible_width ||
+ ntohl(state->header.height) != state->visible_height)
+ return -EINVAL;
+
+ flags = ntohl(state->header.flags);
+
+ if (version >= 2) {
+ if ((flags & FWHT_FL_PIXENC_MSK) != info->pixenc)
+ return -EINVAL;
+ components_num = 1 + ((flags & FWHT_FL_COMPONENTS_NUM_MSK) >>
+ FWHT_FL_COMPONENTS_NUM_OFFSET);
+ }
+
+ if (components_num != info->components_num)
+ return -EINVAL;
+
+ state->colorspace = ntohl(state->header.colorspace);
+ state->xfer_func = ntohl(state->header.xfer_func);
+ state->ycbcr_enc = ntohl(state->header.ycbcr_enc);
+ state->quantization = ntohl(state->header.quantization);
+ cf.rlc_data = (__be16 *)p_in;
+ cf.size = ntohl(state->header.size);
+
+ hdr_width_div = (flags & FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
+ hdr_height_div = (flags & FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
+ if (hdr_width_div != info->width_div ||
+ hdr_height_div != info->height_div)
+ return -EINVAL;
+
+ if (prepare_raw_frame(&dst_rf, info, p_out, dst_size))
+ return -EINVAL;
+ if (info->planes_num == 3) {
+ dst_chroma_stride /= 2;
+ ref_chroma_stride /= 2;
+ }
+ if (info->id == V4L2_PIX_FMT_NV24 ||
+ info->id == V4L2_PIX_FMT_NV42) {
+ dst_chroma_stride *= 2;
+ ref_chroma_stride *= 2;
+ }
+
+
+ ref_size = state->ref_stride * state->coded_height;
+
+ if (prepare_raw_frame(&state->ref_frame, info, state->ref_frame.buf,
+ ref_size))
+ return -EINVAL;
+
+ if (!fwht_decode_frame(&cf, flags, components_num,
+ state->visible_width, state->visible_height,
+ &state->ref_frame, state->ref_stride, ref_chroma_stride,
+ &dst_rf, state->stride, dst_chroma_stride))
+ return -EINVAL;
+ return 0;
+}
diff --git a/drivers/media/test-drivers/vicodec/codec-v4l2-fwht.h b/drivers/media/test-drivers/vicodec/codec-v4l2-fwht.h
new file mode 100644
index 0000000..1a0d2a9
--- /dev/null
+++ b/drivers/media/test-drivers/vicodec/codec-v4l2-fwht.h
@@ -0,0 +1,64 @@
+/* SPDX-License-Identifier: LGPL-2.1 */
+/*
+ * Copyright 2018 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef CODEC_V4L2_FWHT_H
+#define CODEC_V4L2_FWHT_H
+
+#include "codec-fwht.h"
+
+struct v4l2_fwht_pixfmt_info {
+ u32 id;
+ unsigned int bytesperline_mult;
+ unsigned int sizeimage_mult;
+ unsigned int sizeimage_div;
+ unsigned int luma_alpha_step;
+ unsigned int chroma_step;
+ /* Chroma plane subsampling */
+ unsigned int width_div;
+ unsigned int height_div;
+ unsigned int components_num;
+ unsigned int planes_num;
+ unsigned int pixenc;
+};
+
+struct v4l2_fwht_state {
+ const struct v4l2_fwht_pixfmt_info *info;
+ unsigned int visible_width;
+ unsigned int visible_height;
+ unsigned int coded_width;
+ unsigned int coded_height;
+ unsigned int stride;
+ unsigned int ref_stride;
+ unsigned int gop_size;
+ unsigned int gop_cnt;
+ u16 i_frame_qp;
+ u16 p_frame_qp;
+
+ enum v4l2_colorspace colorspace;
+ enum v4l2_ycbcr_encoding ycbcr_enc;
+ enum v4l2_xfer_func xfer_func;
+ enum v4l2_quantization quantization;
+
+ struct fwht_raw_frame ref_frame;
+ struct fwht_cframe_hdr header;
+ u8 *compressed_frame;
+ u64 ref_frame_ts;
+};
+
+const struct v4l2_fwht_pixfmt_info *v4l2_fwht_find_pixfmt(u32 pixelformat);
+const struct v4l2_fwht_pixfmt_info *v4l2_fwht_get_pixfmt(u32 idx);
+bool v4l2_fwht_validate_fmt(const struct v4l2_fwht_pixfmt_info *info,
+ u32 width_div, u32 height_div, u32 components_num,
+ u32 pixenc);
+const struct v4l2_fwht_pixfmt_info *v4l2_fwht_find_nth_fmt(u32 width_div,
+ u32 height_div,
+ u32 components_num,
+ u32 pixenc,
+ unsigned int start_idx);
+
+int v4l2_fwht_encode(struct v4l2_fwht_state *state, u8 *p_in, u8 *p_out);
+int v4l2_fwht_decode(struct v4l2_fwht_state *state, u8 *p_in, u8 *p_out);
+
+#endif
diff --git a/drivers/media/test-drivers/vicodec/vicodec-core.c b/drivers/media/test-drivers/vicodec/vicodec-core.c
new file mode 100644
index 0000000..0e11568
--- /dev/null
+++ b/drivers/media/test-drivers/vicodec/vicodec-core.c
@@ -0,0 +1,2242 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * A virtual codec example device.
+ *
+ * Copyright 2018 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This is a virtual codec device driver for testing the codec framework.
+ * It simulates a device that uses memory buffers for both source and
+ * destination and encodes or decodes the data.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include <linux/platform_device.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "codec-v4l2-fwht.h"
+
+MODULE_DESCRIPTION("Virtual codec device");
+MODULE_AUTHOR("Hans Verkuil <hans.verkuil@cisco.com>");
+MODULE_LICENSE("GPL v2");
+
+static bool multiplanar;
+module_param(multiplanar, bool, 0444);
+MODULE_PARM_DESC(multiplanar,
+ " use multi-planar API instead of single-planar API");
+
+static unsigned int debug;
+module_param(debug, uint, 0644);
+MODULE_PARM_DESC(debug, " activates debug info");
+
+#define VICODEC_NAME "vicodec"
+#define MAX_WIDTH 4096U
+#define MIN_WIDTH 640U
+#define MAX_HEIGHT 2160U
+#define MIN_HEIGHT 360U
+
+#define dprintk(dev, fmt, arg...) \
+ v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
+
+
+struct pixfmt_info {
+ u32 id;
+ unsigned int bytesperline_mult;
+ unsigned int sizeimage_mult;
+ unsigned int sizeimage_div;
+ unsigned int luma_step;
+ unsigned int chroma_step;
+ /* Chroma plane subsampling */
+ unsigned int width_div;
+ unsigned int height_div;
+};
+
+static const struct v4l2_fwht_pixfmt_info pixfmt_fwht = {
+ V4L2_PIX_FMT_FWHT, 0, 3, 1, 1, 1, 1, 1, 0, 1
+};
+
+static const struct v4l2_fwht_pixfmt_info pixfmt_stateless_fwht = {
+ V4L2_PIX_FMT_FWHT_STATELESS, 0, 3, 1, 1, 1, 1, 1, 0, 1
+};
+
+static void vicodec_dev_release(struct device *dev)
+{
+}
+
+static struct platform_device vicodec_pdev = {
+ .name = VICODEC_NAME,
+ .dev.release = vicodec_dev_release,
+};
+
+/* Per-queue, driver-specific private data */
+struct vicodec_q_data {
+ unsigned int coded_width;
+ unsigned int coded_height;
+ unsigned int visible_width;
+ unsigned int visible_height;
+ unsigned int sizeimage;
+ unsigned int vb2_sizeimage;
+ unsigned int sequence;
+ const struct v4l2_fwht_pixfmt_info *info;
+};
+
+enum {
+ V4L2_M2M_SRC = 0,
+ V4L2_M2M_DST = 1,
+};
+
+struct vicodec_dev_instance {
+ struct video_device vfd;
+ struct mutex mutex;
+ spinlock_t lock;
+ struct v4l2_m2m_dev *m2m_dev;
+};
+
+struct vicodec_dev {
+ struct v4l2_device v4l2_dev;
+ struct vicodec_dev_instance stateful_enc;
+ struct vicodec_dev_instance stateful_dec;
+ struct vicodec_dev_instance stateless_dec;
+#ifdef CONFIG_MEDIA_CONTROLLER
+ struct media_device mdev;
+#endif
+
+};
+
+struct vicodec_ctx {
+ struct v4l2_fh fh;
+ struct vicodec_dev *dev;
+ bool is_enc;
+ bool is_stateless;
+ spinlock_t *lock;
+
+ struct v4l2_ctrl_handler hdl;
+
+ /* Source and destination queue data */
+ struct vicodec_q_data q_data[2];
+ struct v4l2_fwht_state state;
+
+ u32 cur_buf_offset;
+ u32 comp_max_size;
+ u32 comp_size;
+ u32 header_size;
+ u32 comp_magic_cnt;
+ bool comp_has_frame;
+ bool comp_has_next_frame;
+ bool first_source_change_sent;
+ bool source_changed;
+};
+
+static const struct v4l2_event vicodec_eos_event = {
+ .type = V4L2_EVENT_EOS
+};
+
+static inline struct vicodec_ctx *file2ctx(struct file *file)
+{
+ return container_of(file->private_data, struct vicodec_ctx, fh);
+}
+
+static struct vicodec_q_data *get_q_data(struct vicodec_ctx *ctx,
+ enum v4l2_buf_type type)
+{
+ switch (type) {
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+ return &ctx->q_data[V4L2_M2M_SRC];
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+ return &ctx->q_data[V4L2_M2M_DST];
+ default:
+ break;
+ }
+ return NULL;
+}
+
+static void copy_cap_to_ref(const u8 *cap, const struct v4l2_fwht_pixfmt_info *info,
+ struct v4l2_fwht_state *state)
+{
+ int plane_idx;
+ u8 *p_ref = state->ref_frame.buf;
+ unsigned int cap_stride = state->stride;
+ unsigned int ref_stride = state->ref_stride;
+
+ for (plane_idx = 0; plane_idx < info->planes_num; plane_idx++) {
+ int i;
+ unsigned int h_div = (plane_idx == 1 || plane_idx == 2) ?
+ info->height_div : 1;
+ const u8 *row_cap = cap;
+ u8 *row_ref = p_ref;
+
+ if (info->planes_num == 3 && plane_idx == 1) {
+ cap_stride /= 2;
+ ref_stride /= 2;
+ }
+
+ if (plane_idx == 1 &&
+ (info->id == V4L2_PIX_FMT_NV24 ||
+ info->id == V4L2_PIX_FMT_NV42)) {
+ cap_stride *= 2;
+ ref_stride *= 2;
+ }
+
+ for (i = 0; i < state->visible_height / h_div; i++) {
+ memcpy(row_ref, row_cap, ref_stride);
+ row_ref += ref_stride;
+ row_cap += cap_stride;
+ }
+ cap += cap_stride * (state->coded_height / h_div);
+ p_ref += ref_stride * (state->coded_height / h_div);
+ }
+}
+
+static bool validate_by_version(unsigned int flags, unsigned int version)
+{
+ if (!version || version > FWHT_VERSION)
+ return false;
+
+ if (version >= 2) {
+ unsigned int components_num = 1 +
+ ((flags & FWHT_FL_COMPONENTS_NUM_MSK) >>
+ FWHT_FL_COMPONENTS_NUM_OFFSET);
+ unsigned int pixenc = flags & FWHT_FL_PIXENC_MSK;
+
+ if (components_num == 0 || components_num > 4 || !pixenc)
+ return false;
+ }
+ return true;
+}
+
+static bool validate_stateless_params_flags(const struct v4l2_ctrl_fwht_params *params,
+ const struct v4l2_fwht_pixfmt_info *cur_info)
+{
+ unsigned int width_div =
+ (params->flags & FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
+ unsigned int height_div =
+ (params->flags & FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
+ unsigned int components_num = 3;
+ unsigned int pixenc = 0;
+
+ if (params->version < 3)
+ return false;
+
+ components_num = 1 + ((params->flags & FWHT_FL_COMPONENTS_NUM_MSK) >>
+ FWHT_FL_COMPONENTS_NUM_OFFSET);
+ pixenc = (params->flags & FWHT_FL_PIXENC_MSK);
+ if (v4l2_fwht_validate_fmt(cur_info, width_div, height_div,
+ components_num, pixenc))
+ return true;
+ return false;
+}
+
+
+static void update_state_from_header(struct vicodec_ctx *ctx)
+{
+ const struct fwht_cframe_hdr *p_hdr = &ctx->state.header;
+
+ ctx->state.visible_width = ntohl(p_hdr->width);
+ ctx->state.visible_height = ntohl(p_hdr->height);
+ ctx->state.colorspace = ntohl(p_hdr->colorspace);
+ ctx->state.xfer_func = ntohl(p_hdr->xfer_func);
+ ctx->state.ycbcr_enc = ntohl(p_hdr->ycbcr_enc);
+ ctx->state.quantization = ntohl(p_hdr->quantization);
+}
+
+static int device_process(struct vicodec_ctx *ctx,
+ struct vb2_v4l2_buffer *src_vb,
+ struct vb2_v4l2_buffer *dst_vb)
+{
+ struct vicodec_dev *dev = ctx->dev;
+ struct v4l2_fwht_state *state = &ctx->state;
+ u8 *p_src, *p_dst;
+ int ret = 0;
+
+ if (ctx->is_enc || ctx->is_stateless)
+ p_src = vb2_plane_vaddr(&src_vb->vb2_buf, 0);
+ else
+ p_src = state->compressed_frame;
+
+ if (ctx->is_stateless) {
+ struct media_request *src_req = src_vb->vb2_buf.req_obj.req;
+
+ ret = v4l2_ctrl_request_setup(src_req, &ctx->hdl);
+ if (ret)
+ return ret;
+ update_state_from_header(ctx);
+
+ ctx->state.header.size =
+ htonl(vb2_get_plane_payload(&src_vb->vb2_buf, 0));
+ /*
+ * set the reference buffer from the reference timestamp
+ * only if this is a P-frame
+ */
+ if (!(ntohl(ctx->state.header.flags) & FWHT_FL_I_FRAME)) {
+ struct vb2_buffer *ref_vb2_buf;
+ int ref_buf_idx;
+ struct vb2_queue *vq_cap =
+ v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+ V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+ ref_buf_idx = vb2_find_timestamp(vq_cap,
+ ctx->state.ref_frame_ts, 0);
+ if (ref_buf_idx < 0)
+ return -EINVAL;
+
+ ref_vb2_buf = vq_cap->bufs[ref_buf_idx];
+ if (ref_vb2_buf->state == VB2_BUF_STATE_ERROR)
+ ret = -EINVAL;
+ ctx->state.ref_frame.buf =
+ vb2_plane_vaddr(ref_vb2_buf, 0);
+ } else {
+ ctx->state.ref_frame.buf = NULL;
+ }
+ }
+ p_dst = vb2_plane_vaddr(&dst_vb->vb2_buf, 0);
+ if (!p_src || !p_dst) {
+ v4l2_err(&dev->v4l2_dev,
+ "Acquiring kernel pointers to buffers failed\n");
+ return -EFAULT;
+ }
+
+ if (ctx->is_enc) {
+ struct vicodec_q_data *q_src;
+ int comp_sz_or_errcode;
+
+ q_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+ state->info = q_src->info;
+ comp_sz_or_errcode = v4l2_fwht_encode(state, p_src, p_dst);
+ if (comp_sz_or_errcode < 0)
+ return comp_sz_or_errcode;
+ vb2_set_plane_payload(&dst_vb->vb2_buf, 0, comp_sz_or_errcode);
+ } else {
+ struct vicodec_q_data *q_dst;
+ unsigned int comp_frame_size = ntohl(ctx->state.header.size);
+
+ q_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+ if (comp_frame_size > ctx->comp_max_size)
+ return -EINVAL;
+ state->info = q_dst->info;
+ ret = v4l2_fwht_decode(state, p_src, p_dst);
+ if (ret < 0)
+ return ret;
+ if (!ctx->is_stateless)
+ copy_cap_to_ref(p_dst, ctx->state.info, &ctx->state);
+
+ vb2_set_plane_payload(&dst_vb->vb2_buf, 0, q_dst->sizeimage);
+ if (ntohl(ctx->state.header.flags) & FWHT_FL_I_FRAME)
+ dst_vb->flags |= V4L2_BUF_FLAG_KEYFRAME;
+ else
+ dst_vb->flags |= V4L2_BUF_FLAG_PFRAME;
+ }
+ return ret;
+}
+
+/*
+ * mem2mem callbacks
+ */
+static enum vb2_buffer_state get_next_header(struct vicodec_ctx *ctx,
+ u8 **pp, u32 sz)
+{
+ static const u8 magic[] = {
+ 0x4f, 0x4f, 0x4f, 0x4f, 0xff, 0xff, 0xff, 0xff
+ };
+ u8 *p = *pp;
+ u32 state;
+ u8 *header = (u8 *)&ctx->state.header;
+
+ state = VB2_BUF_STATE_DONE;
+
+ if (!ctx->header_size) {
+ state = VB2_BUF_STATE_ERROR;
+ for (; p < *pp + sz; p++) {
+ u32 copy;
+
+ p = memchr(p, magic[ctx->comp_magic_cnt],
+ *pp + sz - p);
+ if (!p) {
+ ctx->comp_magic_cnt = 0;
+ p = *pp + sz;
+ break;
+ }
+ copy = sizeof(magic) - ctx->comp_magic_cnt;
+ if (*pp + sz - p < copy)
+ copy = *pp + sz - p;
+
+ memcpy(header + ctx->comp_magic_cnt, p, copy);
+ ctx->comp_magic_cnt += copy;
+ if (!memcmp(header, magic, ctx->comp_magic_cnt)) {
+ p += copy;
+ state = VB2_BUF_STATE_DONE;
+ break;
+ }
+ ctx->comp_magic_cnt = 0;
+ }
+ if (ctx->comp_magic_cnt < sizeof(magic)) {
+ *pp = p;
+ return state;
+ }
+ ctx->header_size = sizeof(magic);
+ }
+
+ if (ctx->header_size < sizeof(struct fwht_cframe_hdr)) {
+ u32 copy = sizeof(struct fwht_cframe_hdr) - ctx->header_size;
+
+ if (*pp + sz - p < copy)
+ copy = *pp + sz - p;
+
+ memcpy(header + ctx->header_size, p, copy);
+ p += copy;
+ ctx->header_size += copy;
+ }
+ *pp = p;
+ return state;
+}
+
+/* device_run() - prepares and starts the device */
+static void device_run(void *priv)
+{
+ struct vicodec_ctx *ctx = priv;
+ struct vicodec_dev *dev = ctx->dev;
+ struct vb2_v4l2_buffer *src_buf, *dst_buf;
+ struct vicodec_q_data *q_src, *q_dst;
+ u32 state;
+ struct media_request *src_req;
+
+ src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+ dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+ src_req = src_buf->vb2_buf.req_obj.req;
+
+ q_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+ q_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+ state = VB2_BUF_STATE_DONE;
+ if (device_process(ctx, src_buf, dst_buf))
+ state = VB2_BUF_STATE_ERROR;
+ else
+ dst_buf->sequence = q_dst->sequence++;
+ dst_buf->flags &= ~V4L2_BUF_FLAG_LAST;
+ v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, false);
+
+ spin_lock(ctx->lock);
+ if (!ctx->comp_has_next_frame &&
+ v4l2_m2m_is_last_draining_src_buf(ctx->fh.m2m_ctx, src_buf)) {
+ dst_buf->flags |= V4L2_BUF_FLAG_LAST;
+ v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
+ v4l2_m2m_mark_stopped(ctx->fh.m2m_ctx);
+ }
+ if (ctx->is_enc || ctx->is_stateless) {
+ src_buf->sequence = q_src->sequence++;
+ src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+ v4l2_m2m_buf_done(src_buf, state);
+ } else if (vb2_get_plane_payload(&src_buf->vb2_buf, 0) == ctx->cur_buf_offset) {
+ src_buf->sequence = q_src->sequence++;
+ src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+ v4l2_m2m_buf_done(src_buf, state);
+ ctx->cur_buf_offset = 0;
+ ctx->comp_has_next_frame = false;
+ }
+ v4l2_m2m_buf_done(dst_buf, state);
+
+ ctx->comp_size = 0;
+ ctx->header_size = 0;
+ ctx->comp_magic_cnt = 0;
+ ctx->comp_has_frame = false;
+ spin_unlock(ctx->lock);
+ if (ctx->is_stateless && src_req)
+ v4l2_ctrl_request_complete(src_req, &ctx->hdl);
+
+ if (ctx->is_enc)
+ v4l2_m2m_job_finish(dev->stateful_enc.m2m_dev, ctx->fh.m2m_ctx);
+ else if (ctx->is_stateless)
+ v4l2_m2m_job_finish(dev->stateless_dec.m2m_dev,
+ ctx->fh.m2m_ctx);
+ else
+ v4l2_m2m_job_finish(dev->stateful_dec.m2m_dev, ctx->fh.m2m_ctx);
+}
+
+static void job_remove_src_buf(struct vicodec_ctx *ctx, u32 state)
+{
+ struct vb2_v4l2_buffer *src_buf;
+ struct vicodec_q_data *q_src;
+
+ q_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+ spin_lock(ctx->lock);
+ src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+ src_buf->sequence = q_src->sequence++;
+ v4l2_m2m_buf_done(src_buf, state);
+ ctx->cur_buf_offset = 0;
+ spin_unlock(ctx->lock);
+}
+
+static const struct v4l2_fwht_pixfmt_info *
+info_from_header(const struct fwht_cframe_hdr *p_hdr)
+{
+ unsigned int flags = ntohl(p_hdr->flags);
+ unsigned int width_div = (flags & FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
+ unsigned int height_div = (flags & FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
+ unsigned int components_num = 3;
+ unsigned int pixenc = 0;
+ unsigned int version = ntohl(p_hdr->version);
+
+ if (version >= 2) {
+ components_num = 1 + ((flags & FWHT_FL_COMPONENTS_NUM_MSK) >>
+ FWHT_FL_COMPONENTS_NUM_OFFSET);
+ pixenc = (flags & FWHT_FL_PIXENC_MSK);
+ }
+ return v4l2_fwht_find_nth_fmt(width_div, height_div,
+ components_num, pixenc, 0);
+}
+
+static bool is_header_valid(const struct fwht_cframe_hdr *p_hdr)
+{
+ const struct v4l2_fwht_pixfmt_info *info;
+ unsigned int w = ntohl(p_hdr->width);
+ unsigned int h = ntohl(p_hdr->height);
+ unsigned int version = ntohl(p_hdr->version);
+ unsigned int flags = ntohl(p_hdr->flags);
+
+ if (w < MIN_WIDTH || w > MAX_WIDTH || h < MIN_HEIGHT || h > MAX_HEIGHT)
+ return false;
+
+ if (!validate_by_version(flags, version))
+ return false;
+
+ info = info_from_header(p_hdr);
+ if (!info)
+ return false;
+ return true;
+}
+
+static void update_capture_data_from_header(struct vicodec_ctx *ctx)
+{
+ struct vicodec_q_data *q_dst = get_q_data(ctx,
+ V4L2_BUF_TYPE_VIDEO_CAPTURE);
+ const struct fwht_cframe_hdr *p_hdr = &ctx->state.header;
+ const struct v4l2_fwht_pixfmt_info *info = info_from_header(p_hdr);
+ unsigned int flags = ntohl(p_hdr->flags);
+ unsigned int hdr_width_div = (flags & FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
+ unsigned int hdr_height_div = (flags & FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
+
+ /*
+ * This function should not be used by a stateless codec since
+ * it changes values in q_data that are not request specific
+ */
+ WARN_ON(ctx->is_stateless);
+
+ q_dst->info = info;
+ q_dst->visible_width = ntohl(p_hdr->width);
+ q_dst->visible_height = ntohl(p_hdr->height);
+ q_dst->coded_width = vic_round_dim(q_dst->visible_width, hdr_width_div);
+ q_dst->coded_height = vic_round_dim(q_dst->visible_height,
+ hdr_height_div);
+
+ q_dst->sizeimage = q_dst->coded_width * q_dst->coded_height *
+ q_dst->info->sizeimage_mult / q_dst->info->sizeimage_div;
+ ctx->state.colorspace = ntohl(p_hdr->colorspace);
+
+ ctx->state.xfer_func = ntohl(p_hdr->xfer_func);
+ ctx->state.ycbcr_enc = ntohl(p_hdr->ycbcr_enc);
+ ctx->state.quantization = ntohl(p_hdr->quantization);
+}
+
+static void set_last_buffer(struct vb2_v4l2_buffer *dst_buf,
+ const struct vb2_v4l2_buffer *src_buf,
+ struct vicodec_ctx *ctx)
+{
+ struct vicodec_q_data *q_dst = get_q_data(ctx,
+ V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+ vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
+ dst_buf->sequence = q_dst->sequence++;
+
+ v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, !ctx->is_enc);
+ dst_buf->flags |= V4L2_BUF_FLAG_LAST;
+ v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+}
+
+static int job_ready(void *priv)
+{
+ static const u8 magic[] = {
+ 0x4f, 0x4f, 0x4f, 0x4f, 0xff, 0xff, 0xff, 0xff
+ };
+ struct vicodec_ctx *ctx = priv;
+ struct vb2_v4l2_buffer *src_buf;
+ u8 *p_src;
+ u8 *p;
+ u32 sz;
+ u32 state;
+ struct vicodec_q_data *q_dst = get_q_data(ctx,
+ V4L2_BUF_TYPE_VIDEO_CAPTURE);
+ unsigned int flags;
+ unsigned int hdr_width_div;
+ unsigned int hdr_height_div;
+ unsigned int max_to_copy;
+ unsigned int comp_frame_size;
+
+ if (ctx->source_changed)
+ return 0;
+ if (ctx->is_stateless || ctx->is_enc || ctx->comp_has_frame)
+ return 1;
+
+restart:
+ ctx->comp_has_next_frame = false;
+ src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+ if (!src_buf)
+ return 0;
+ p_src = vb2_plane_vaddr(&src_buf->vb2_buf, 0);
+ sz = vb2_get_plane_payload(&src_buf->vb2_buf, 0);
+ p = p_src + ctx->cur_buf_offset;
+
+ state = VB2_BUF_STATE_DONE;
+
+ if (ctx->header_size < sizeof(struct fwht_cframe_hdr)) {
+ state = get_next_header(ctx, &p, p_src + sz - p);
+ if (ctx->header_size < sizeof(struct fwht_cframe_hdr)) {
+ if (v4l2_m2m_is_last_draining_src_buf(ctx->fh.m2m_ctx,
+ src_buf))
+ return 1;
+ job_remove_src_buf(ctx, state);
+ goto restart;
+ }
+ }
+
+ comp_frame_size = ntohl(ctx->state.header.size);
+
+ /*
+ * The current scanned frame might be the first frame of a new
+ * resolution so its size might be larger than ctx->comp_max_size.
+ * In that case it is copied up to the current buffer capacity and
+ * the copy will continue after allocating new large enough buffer
+ * when restreaming
+ */
+ max_to_copy = min(comp_frame_size, ctx->comp_max_size);
+
+ if (ctx->comp_size < max_to_copy) {
+ u32 copy = max_to_copy - ctx->comp_size;
+
+ if (copy > p_src + sz - p)
+ copy = p_src + sz - p;
+
+ memcpy(ctx->state.compressed_frame + ctx->comp_size,
+ p, copy);
+ p += copy;
+ ctx->comp_size += copy;
+ if (ctx->comp_size < max_to_copy) {
+ if (v4l2_m2m_is_last_draining_src_buf(ctx->fh.m2m_ctx,
+ src_buf))
+ return 1;
+ job_remove_src_buf(ctx, state);
+ goto restart;
+ }
+ }
+ ctx->cur_buf_offset = p - p_src;
+ if (ctx->comp_size == comp_frame_size)
+ ctx->comp_has_frame = true;
+ ctx->comp_has_next_frame = false;
+ if (ctx->comp_has_frame && sz - ctx->cur_buf_offset >=
+ sizeof(struct fwht_cframe_hdr)) {
+ struct fwht_cframe_hdr *p_hdr = (struct fwht_cframe_hdr *)p;
+ u32 frame_size = ntohl(p_hdr->size);
+ u32 remaining = sz - ctx->cur_buf_offset - sizeof(*p_hdr);
+
+ if (!memcmp(p, magic, sizeof(magic)))
+ ctx->comp_has_next_frame = remaining >= frame_size;
+ }
+ /*
+ * if the header is invalid the device_run will just drop the frame
+ * with an error
+ */
+ if (!is_header_valid(&ctx->state.header) && ctx->comp_has_frame)
+ return 1;
+ flags = ntohl(ctx->state.header.flags);
+ hdr_width_div = (flags & FWHT_FL_CHROMA_FULL_WIDTH) ? 1 : 2;
+ hdr_height_div = (flags & FWHT_FL_CHROMA_FULL_HEIGHT) ? 1 : 2;
+
+ if (ntohl(ctx->state.header.width) != q_dst->visible_width ||
+ ntohl(ctx->state.header.height) != q_dst->visible_height ||
+ !q_dst->info ||
+ hdr_width_div != q_dst->info->width_div ||
+ hdr_height_div != q_dst->info->height_div) {
+ static const struct v4l2_event rs_event = {
+ .type = V4L2_EVENT_SOURCE_CHANGE,
+ .u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+ };
+
+ struct vb2_v4l2_buffer *dst_buf =
+ v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+
+ update_capture_data_from_header(ctx);
+ v4l2_event_queue_fh(&ctx->fh, &rs_event);
+ set_last_buffer(dst_buf, src_buf, ctx);
+ ctx->source_changed = true;
+ return 0;
+ }
+ return 1;
+}
+
+/*
+ * video ioctls
+ */
+
+static const struct v4l2_fwht_pixfmt_info *find_fmt(u32 fmt)
+{
+ const struct v4l2_fwht_pixfmt_info *info =
+ v4l2_fwht_find_pixfmt(fmt);
+
+ if (!info)
+ info = v4l2_fwht_get_pixfmt(0);
+ return info;
+}
+
+static int vidioc_querycap(struct file *file, void *priv,
+ struct v4l2_capability *cap)
+{
+ strscpy(cap->driver, VICODEC_NAME, sizeof(cap->driver));
+ strscpy(cap->card, VICODEC_NAME, sizeof(cap->card));
+ snprintf(cap->bus_info, sizeof(cap->bus_info),
+ "platform:%s", VICODEC_NAME);
+ return 0;
+}
+
+static int enum_fmt(struct v4l2_fmtdesc *f, struct vicodec_ctx *ctx,
+ bool is_out)
+{
+ bool is_uncomp = (ctx->is_enc && is_out) || (!ctx->is_enc && !is_out);
+
+ if (V4L2_TYPE_IS_MULTIPLANAR(f->type) && !multiplanar)
+ return -EINVAL;
+ if (!V4L2_TYPE_IS_MULTIPLANAR(f->type) && multiplanar)
+ return -EINVAL;
+
+ if (is_uncomp) {
+ const struct v4l2_fwht_pixfmt_info *info =
+ get_q_data(ctx, f->type)->info;
+
+ if (ctx->is_enc ||
+ !vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q))
+ info = v4l2_fwht_get_pixfmt(f->index);
+ else
+ info = v4l2_fwht_find_nth_fmt(info->width_div,
+ info->height_div,
+ info->components_num,
+ info->pixenc,
+ f->index);
+ if (!info)
+ return -EINVAL;
+ f->pixelformat = info->id;
+ } else {
+ if (f->index)
+ return -EINVAL;
+ f->pixelformat = ctx->is_stateless ?
+ V4L2_PIX_FMT_FWHT_STATELESS : V4L2_PIX_FMT_FWHT;
+ if (!ctx->is_enc && !ctx->is_stateless)
+ f->flags = V4L2_FMT_FLAG_DYN_RESOLUTION |
+ V4L2_FMT_FLAG_CONTINUOUS_BYTESTREAM;
+ }
+ return 0;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ struct vicodec_ctx *ctx = file2ctx(file);
+
+ return enum_fmt(f, ctx, false);
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ struct vicodec_ctx *ctx = file2ctx(file);
+
+ return enum_fmt(f, ctx, true);
+}
+
+static int vidioc_g_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
+{
+ struct vb2_queue *vq;
+ struct vicodec_q_data *q_data;
+ struct v4l2_pix_format_mplane *pix_mp;
+ struct v4l2_pix_format *pix;
+ const struct v4l2_fwht_pixfmt_info *info;
+
+ vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+ if (!vq)
+ return -EINVAL;
+
+ q_data = get_q_data(ctx, f->type);
+ info = q_data->info;
+
+ switch (f->type) {
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+ if (multiplanar)
+ return -EINVAL;
+ pix = &f->fmt.pix;
+ pix->width = q_data->coded_width;
+ pix->height = q_data->coded_height;
+ pix->field = V4L2_FIELD_NONE;
+ pix->pixelformat = info->id;
+ pix->bytesperline = q_data->coded_width *
+ info->bytesperline_mult;
+ pix->sizeimage = q_data->sizeimage;
+ pix->colorspace = ctx->state.colorspace;
+ pix->xfer_func = ctx->state.xfer_func;
+ pix->ycbcr_enc = ctx->state.ycbcr_enc;
+ pix->quantization = ctx->state.quantization;
+ break;
+
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+ if (!multiplanar)
+ return -EINVAL;
+ pix_mp = &f->fmt.pix_mp;
+ pix_mp->width = q_data->coded_width;
+ pix_mp->height = q_data->coded_height;
+ pix_mp->field = V4L2_FIELD_NONE;
+ pix_mp->pixelformat = info->id;
+ pix_mp->num_planes = 1;
+ pix_mp->plane_fmt[0].bytesperline =
+ q_data->coded_width * info->bytesperline_mult;
+ pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage;
+ pix_mp->colorspace = ctx->state.colorspace;
+ pix_mp->xfer_func = ctx->state.xfer_func;
+ pix_mp->ycbcr_enc = ctx->state.ycbcr_enc;
+ pix_mp->quantization = ctx->state.quantization;
+ memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
+ memset(pix_mp->plane_fmt[0].reserved, 0,
+ sizeof(pix_mp->plane_fmt[0].reserved));
+ break;
+ default:
+ return -EINVAL;
+ }
+ return 0;
+}
+
+static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ return vidioc_g_fmt(file2ctx(file), f);
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ return vidioc_g_fmt(file2ctx(file), f);
+}
+
+static int vidioc_try_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
+{
+ struct v4l2_pix_format_mplane *pix_mp;
+ struct v4l2_pix_format *pix;
+ struct v4l2_plane_pix_format *plane;
+ const struct v4l2_fwht_pixfmt_info *info = ctx->is_stateless ?
+ &pixfmt_stateless_fwht : &pixfmt_fwht;
+
+ switch (f->type) {
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+ pix = &f->fmt.pix;
+ if (pix->pixelformat != V4L2_PIX_FMT_FWHT &&
+ pix->pixelformat != V4L2_PIX_FMT_FWHT_STATELESS)
+ info = find_fmt(pix->pixelformat);
+
+ pix->width = clamp(pix->width, MIN_WIDTH, MAX_WIDTH);
+ pix->width = vic_round_dim(pix->width, info->width_div);
+
+ pix->height = clamp(pix->height, MIN_HEIGHT, MAX_HEIGHT);
+ pix->height = vic_round_dim(pix->height, info->height_div);
+
+ pix->field = V4L2_FIELD_NONE;
+ pix->bytesperline =
+ pix->width * info->bytesperline_mult;
+ pix->sizeimage = pix->width * pix->height *
+ info->sizeimage_mult / info->sizeimage_div;
+ if (pix->pixelformat == V4L2_PIX_FMT_FWHT)
+ pix->sizeimage += sizeof(struct fwht_cframe_hdr);
+ break;
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+ pix_mp = &f->fmt.pix_mp;
+ plane = pix_mp->plane_fmt;
+ if (pix_mp->pixelformat != V4L2_PIX_FMT_FWHT &&
+ pix_mp->pixelformat != V4L2_PIX_FMT_FWHT_STATELESS)
+ info = find_fmt(pix_mp->pixelformat);
+ pix_mp->num_planes = 1;
+
+ pix_mp->width = clamp(pix_mp->width, MIN_WIDTH, MAX_WIDTH);
+ pix_mp->width = vic_round_dim(pix_mp->width, info->width_div);
+
+ pix_mp->height = clamp(pix_mp->height, MIN_HEIGHT, MAX_HEIGHT);
+ pix_mp->height = vic_round_dim(pix_mp->height,
+ info->height_div);
+
+ pix_mp->field = V4L2_FIELD_NONE;
+ plane->bytesperline =
+ pix_mp->width * info->bytesperline_mult;
+ plane->sizeimage = pix_mp->width * pix_mp->height *
+ info->sizeimage_mult / info->sizeimage_div;
+ if (pix_mp->pixelformat == V4L2_PIX_FMT_FWHT)
+ plane->sizeimage += sizeof(struct fwht_cframe_hdr);
+ memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
+ memset(plane->reserved, 0, sizeof(plane->reserved));
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vicodec_ctx *ctx = file2ctx(file);
+ struct v4l2_pix_format_mplane *pix_mp;
+ struct v4l2_pix_format *pix;
+
+ switch (f->type) {
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+ if (multiplanar)
+ return -EINVAL;
+ pix = &f->fmt.pix;
+ pix->pixelformat = ctx->is_enc ? V4L2_PIX_FMT_FWHT :
+ find_fmt(f->fmt.pix.pixelformat)->id;
+ pix->colorspace = ctx->state.colorspace;
+ pix->xfer_func = ctx->state.xfer_func;
+ pix->ycbcr_enc = ctx->state.ycbcr_enc;
+ pix->quantization = ctx->state.quantization;
+ break;
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+ if (!multiplanar)
+ return -EINVAL;
+ pix_mp = &f->fmt.pix_mp;
+ pix_mp->pixelformat = ctx->is_enc ? V4L2_PIX_FMT_FWHT :
+ find_fmt(pix_mp->pixelformat)->id;
+ pix_mp->colorspace = ctx->state.colorspace;
+ pix_mp->xfer_func = ctx->state.xfer_func;
+ pix_mp->ycbcr_enc = ctx->state.ycbcr_enc;
+ pix_mp->quantization = ctx->state.quantization;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return vidioc_try_fmt(ctx, f);
+}
+
+static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vicodec_ctx *ctx = file2ctx(file);
+ struct v4l2_pix_format_mplane *pix_mp;
+ struct v4l2_pix_format *pix;
+
+ switch (f->type) {
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+ if (multiplanar)
+ return -EINVAL;
+ pix = &f->fmt.pix;
+ if (ctx->is_enc)
+ pix->pixelformat = find_fmt(pix->pixelformat)->id;
+ else if (ctx->is_stateless)
+ pix->pixelformat = V4L2_PIX_FMT_FWHT_STATELESS;
+ else
+ pix->pixelformat = V4L2_PIX_FMT_FWHT;
+ if (!pix->colorspace)
+ pix->colorspace = V4L2_COLORSPACE_REC709;
+ break;
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+ if (!multiplanar)
+ return -EINVAL;
+ pix_mp = &f->fmt.pix_mp;
+ if (ctx->is_enc)
+ pix_mp->pixelformat = find_fmt(pix_mp->pixelformat)->id;
+ else if (ctx->is_stateless)
+ pix_mp->pixelformat = V4L2_PIX_FMT_FWHT_STATELESS;
+ else
+ pix_mp->pixelformat = V4L2_PIX_FMT_FWHT;
+ if (!pix_mp->colorspace)
+ pix_mp->colorspace = V4L2_COLORSPACE_REC709;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return vidioc_try_fmt(ctx, f);
+}
+
+static int vidioc_s_fmt(struct vicodec_ctx *ctx, struct v4l2_format *f)
+{
+ struct vicodec_q_data *q_data;
+ struct vb2_queue *vq;
+ bool fmt_changed = true;
+ struct v4l2_pix_format_mplane *pix_mp;
+ struct v4l2_pix_format *pix;
+
+ vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+ if (!vq)
+ return -EINVAL;
+
+ q_data = get_q_data(ctx, f->type);
+ if (!q_data)
+ return -EINVAL;
+
+ switch (f->type) {
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+ pix = &f->fmt.pix;
+ if (ctx->is_enc && V4L2_TYPE_IS_OUTPUT(f->type))
+ fmt_changed =
+ !q_data->info ||
+ q_data->info->id != pix->pixelformat ||
+ q_data->coded_width != pix->width ||
+ q_data->coded_height != pix->height;
+
+ if (vb2_is_busy(vq) && fmt_changed)
+ return -EBUSY;
+
+ if (pix->pixelformat == V4L2_PIX_FMT_FWHT)
+ q_data->info = &pixfmt_fwht;
+ else if (pix->pixelformat == V4L2_PIX_FMT_FWHT_STATELESS)
+ q_data->info = &pixfmt_stateless_fwht;
+ else
+ q_data->info = find_fmt(pix->pixelformat);
+ q_data->coded_width = pix->width;
+ q_data->coded_height = pix->height;
+ q_data->sizeimage = pix->sizeimage;
+ break;
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE:
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+ pix_mp = &f->fmt.pix_mp;
+ if (ctx->is_enc && V4L2_TYPE_IS_OUTPUT(f->type))
+ fmt_changed =
+ !q_data->info ||
+ q_data->info->id != pix_mp->pixelformat ||
+ q_data->coded_width != pix_mp->width ||
+ q_data->coded_height != pix_mp->height;
+
+ if (vb2_is_busy(vq) && fmt_changed)
+ return -EBUSY;
+
+ if (pix_mp->pixelformat == V4L2_PIX_FMT_FWHT)
+ q_data->info = &pixfmt_fwht;
+ else if (pix_mp->pixelformat == V4L2_PIX_FMT_FWHT_STATELESS)
+ q_data->info = &pixfmt_stateless_fwht;
+ else
+ q_data->info = find_fmt(pix_mp->pixelformat);
+ q_data->coded_width = pix_mp->width;
+ q_data->coded_height = pix_mp->height;
+ q_data->sizeimage = pix_mp->plane_fmt[0].sizeimage;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ dprintk(ctx->dev,
+ "Setting format for type %d, coded wxh: %dx%d, fourcc: 0x%08x\n",
+ f->type, q_data->coded_width, q_data->coded_height,
+ q_data->info->id);
+
+ return 0;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ int ret;
+
+ ret = vidioc_try_fmt_vid_cap(file, priv, f);
+ if (ret)
+ return ret;
+
+ return vidioc_s_fmt(file2ctx(file), f);
+}
+
+static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vicodec_ctx *ctx = file2ctx(file);
+ struct vicodec_q_data *q_data;
+ struct vicodec_q_data *q_data_cap;
+ struct v4l2_pix_format *pix;
+ struct v4l2_pix_format_mplane *pix_mp;
+ u32 coded_w = 0, coded_h = 0;
+ unsigned int size = 0;
+ int ret;
+
+ q_data = get_q_data(ctx, f->type);
+ q_data_cap = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+ ret = vidioc_try_fmt_vid_out(file, priv, f);
+ if (ret)
+ return ret;
+
+ if (ctx->is_enc) {
+ struct vb2_queue *vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+ struct vb2_queue *vq_cap = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+ V4L2_BUF_TYPE_VIDEO_CAPTURE);
+ const struct v4l2_fwht_pixfmt_info *info = ctx->is_stateless ?
+ &pixfmt_stateless_fwht : &pixfmt_fwht;
+
+ if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+ coded_w = f->fmt.pix.width;
+ coded_h = f->fmt.pix.height;
+ } else {
+ coded_w = f->fmt.pix_mp.width;
+ coded_h = f->fmt.pix_mp.height;
+ }
+ if (vb2_is_busy(vq) && (coded_w != q_data->coded_width ||
+ coded_h != q_data->coded_height))
+ return -EBUSY;
+ size = coded_w * coded_h *
+ info->sizeimage_mult / info->sizeimage_div;
+ if (!ctx->is_stateless)
+ size += sizeof(struct fwht_cframe_hdr);
+
+ if (vb2_is_busy(vq_cap) && size > q_data_cap->sizeimage)
+ return -EBUSY;
+ }
+
+ ret = vidioc_s_fmt(file2ctx(file), f);
+ if (!ret) {
+ if (ctx->is_enc) {
+ q_data->visible_width = coded_w;
+ q_data->visible_height = coded_h;
+ q_data_cap->coded_width = coded_w;
+ q_data_cap->coded_height = coded_h;
+ q_data_cap->sizeimage = size;
+ }
+
+ switch (f->type) {
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+ pix = &f->fmt.pix;
+ ctx->state.colorspace = pix->colorspace;
+ ctx->state.xfer_func = pix->xfer_func;
+ ctx->state.ycbcr_enc = pix->ycbcr_enc;
+ ctx->state.quantization = pix->quantization;
+ break;
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE:
+ pix_mp = &f->fmt.pix_mp;
+ ctx->state.colorspace = pix_mp->colorspace;
+ ctx->state.xfer_func = pix_mp->xfer_func;
+ ctx->state.ycbcr_enc = pix_mp->ycbcr_enc;
+ ctx->state.quantization = pix_mp->quantization;
+ break;
+ default:
+ break;
+ }
+ }
+ return ret;
+}
+
+static int vidioc_g_selection(struct file *file, void *priv,
+ struct v4l2_selection *s)
+{
+ struct vicodec_ctx *ctx = file2ctx(file);
+ struct vicodec_q_data *q_data;
+
+ q_data = get_q_data(ctx, s->type);
+ if (!q_data)
+ return -EINVAL;
+ /*
+ * encoder supports only cropping on the OUTPUT buffer
+ * decoder supports only composing on the CAPTURE buffer
+ */
+ if (ctx->is_enc && s->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+ switch (s->target) {
+ case V4L2_SEL_TGT_CROP:
+ s->r.left = 0;
+ s->r.top = 0;
+ s->r.width = q_data->visible_width;
+ s->r.height = q_data->visible_height;
+ return 0;
+ case V4L2_SEL_TGT_CROP_DEFAULT:
+ case V4L2_SEL_TGT_CROP_BOUNDS:
+ s->r.left = 0;
+ s->r.top = 0;
+ s->r.width = q_data->coded_width;
+ s->r.height = q_data->coded_height;
+ return 0;
+ }
+ } else if (!ctx->is_enc && s->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+ switch (s->target) {
+ case V4L2_SEL_TGT_COMPOSE:
+ s->r.left = 0;
+ s->r.top = 0;
+ s->r.width = q_data->visible_width;
+ s->r.height = q_data->visible_height;
+ return 0;
+ case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+ case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+ s->r.left = 0;
+ s->r.top = 0;
+ s->r.width = q_data->coded_width;
+ s->r.height = q_data->coded_height;
+ return 0;
+ }
+ }
+ return -EINVAL;
+}
+
+static int vidioc_s_selection(struct file *file, void *priv,
+ struct v4l2_selection *s)
+{
+ struct vicodec_ctx *ctx = file2ctx(file);
+ struct vicodec_q_data *q_data;
+
+ if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+ return -EINVAL;
+
+ q_data = get_q_data(ctx, s->type);
+ if (!q_data)
+ return -EINVAL;
+
+ if (!ctx->is_enc || s->target != V4L2_SEL_TGT_CROP)
+ return -EINVAL;
+
+ s->r.left = 0;
+ s->r.top = 0;
+ q_data->visible_width = clamp(s->r.width, MIN_WIDTH,
+ q_data->coded_width);
+ s->r.width = q_data->visible_width;
+ q_data->visible_height = clamp(s->r.height, MIN_HEIGHT,
+ q_data->coded_height);
+ s->r.height = q_data->visible_height;
+ return 0;
+}
+
+static int vicodec_encoder_cmd(struct file *file, void *fh,
+ struct v4l2_encoder_cmd *ec)
+{
+ struct vicodec_ctx *ctx = file2ctx(file);
+ int ret;
+
+ ret = v4l2_m2m_ioctl_try_encoder_cmd(file, fh, ec);
+ if (ret < 0)
+ return ret;
+
+ if (!vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q) ||
+ !vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q))
+ return 0;
+
+ ret = v4l2_m2m_ioctl_encoder_cmd(file, fh, ec);
+ if (ret < 0)
+ return ret;
+
+ if (ec->cmd == V4L2_ENC_CMD_STOP &&
+ v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
+ v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
+
+ if (ec->cmd == V4L2_ENC_CMD_START &&
+ v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
+ vb2_clear_last_buffer_dequeued(&ctx->fh.m2m_ctx->cap_q_ctx.q);
+
+ return 0;
+}
+
+static int vicodec_decoder_cmd(struct file *file, void *fh,
+ struct v4l2_decoder_cmd *dc)
+{
+ struct vicodec_ctx *ctx = file2ctx(file);
+ int ret;
+
+ ret = v4l2_m2m_ioctl_try_decoder_cmd(file, fh, dc);
+ if (ret < 0)
+ return ret;
+
+ if (!vb2_is_streaming(&ctx->fh.m2m_ctx->cap_q_ctx.q) ||
+ !vb2_is_streaming(&ctx->fh.m2m_ctx->out_q_ctx.q))
+ return 0;
+
+ ret = v4l2_m2m_ioctl_decoder_cmd(file, fh, dc);
+ if (ret < 0)
+ return ret;
+
+ if (dc->cmd == V4L2_DEC_CMD_STOP &&
+ v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
+ v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
+
+ if (dc->cmd == V4L2_DEC_CMD_START &&
+ v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
+ vb2_clear_last_buffer_dequeued(&ctx->fh.m2m_ctx->cap_q_ctx.q);
+
+ return 0;
+}
+
+static int vicodec_enum_framesizes(struct file *file, void *fh,
+ struct v4l2_frmsizeenum *fsize)
+{
+ switch (fsize->pixel_format) {
+ case V4L2_PIX_FMT_FWHT_STATELESS:
+ break;
+ case V4L2_PIX_FMT_FWHT:
+ break;
+ default:
+ if (find_fmt(fsize->pixel_format)->id == fsize->pixel_format)
+ break;
+ return -EINVAL;
+ }
+
+ if (fsize->index)
+ return -EINVAL;
+
+ fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+
+ fsize->stepwise.min_width = MIN_WIDTH;
+ fsize->stepwise.max_width = MAX_WIDTH;
+ fsize->stepwise.step_width = 8;
+ fsize->stepwise.min_height = MIN_HEIGHT;
+ fsize->stepwise.max_height = MAX_HEIGHT;
+ fsize->stepwise.step_height = 8;
+
+ return 0;
+}
+
+static int vicodec_subscribe_event(struct v4l2_fh *fh,
+ const struct v4l2_event_subscription *sub)
+{
+ struct vicodec_ctx *ctx = container_of(fh, struct vicodec_ctx, fh);
+
+ switch (sub->type) {
+ case V4L2_EVENT_SOURCE_CHANGE:
+ if (ctx->is_enc)
+ return -EINVAL;
+ fallthrough;
+ case V4L2_EVENT_EOS:
+ if (ctx->is_stateless)
+ return -EINVAL;
+ return v4l2_event_subscribe(fh, sub, 0, NULL);
+ default:
+ return v4l2_ctrl_subscribe_event(fh, sub);
+ }
+}
+
+static const struct v4l2_ioctl_ops vicodec_ioctl_ops = {
+ .vidioc_querycap = vidioc_querycap,
+
+ .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+ .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
+ .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
+ .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
+
+ .vidioc_g_fmt_vid_cap_mplane = vidioc_g_fmt_vid_cap,
+ .vidioc_try_fmt_vid_cap_mplane = vidioc_try_fmt_vid_cap,
+ .vidioc_s_fmt_vid_cap_mplane = vidioc_s_fmt_vid_cap,
+
+ .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
+ .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out,
+ .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
+ .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out,
+
+ .vidioc_g_fmt_vid_out_mplane = vidioc_g_fmt_vid_out,
+ .vidioc_try_fmt_vid_out_mplane = vidioc_try_fmt_vid_out,
+ .vidioc_s_fmt_vid_out_mplane = vidioc_s_fmt_vid_out,
+
+ .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs,
+ .vidioc_querybuf = v4l2_m2m_ioctl_querybuf,
+ .vidioc_qbuf = v4l2_m2m_ioctl_qbuf,
+ .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,
+ .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
+ .vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs,
+ .vidioc_expbuf = v4l2_m2m_ioctl_expbuf,
+
+ .vidioc_streamon = v4l2_m2m_ioctl_streamon,
+ .vidioc_streamoff = v4l2_m2m_ioctl_streamoff,
+
+ .vidioc_g_selection = vidioc_g_selection,
+ .vidioc_s_selection = vidioc_s_selection,
+
+ .vidioc_try_encoder_cmd = v4l2_m2m_ioctl_try_encoder_cmd,
+ .vidioc_encoder_cmd = vicodec_encoder_cmd,
+ .vidioc_try_decoder_cmd = v4l2_m2m_ioctl_try_decoder_cmd,
+ .vidioc_decoder_cmd = vicodec_decoder_cmd,
+ .vidioc_enum_framesizes = vicodec_enum_framesizes,
+
+ .vidioc_subscribe_event = vicodec_subscribe_event,
+ .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+
+/*
+ * Queue operations
+ */
+
+static int vicodec_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+ unsigned int *nplanes, unsigned int sizes[],
+ struct device *alloc_devs[])
+{
+ struct vicodec_ctx *ctx = vb2_get_drv_priv(vq);
+ struct vicodec_q_data *q_data = get_q_data(ctx, vq->type);
+ unsigned int size = q_data->sizeimage;
+
+ if (*nplanes)
+ return sizes[0] < size ? -EINVAL : 0;
+
+ *nplanes = 1;
+ sizes[0] = size;
+ q_data->vb2_sizeimage = size;
+ return 0;
+}
+
+static int vicodec_buf_out_validate(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+
+ vbuf->field = V4L2_FIELD_NONE;
+ return 0;
+}
+
+static int vicodec_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+ struct vicodec_q_data *q_data;
+
+ dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
+
+ q_data = get_q_data(ctx, vb->vb2_queue->type);
+ if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
+ if (vbuf->field == V4L2_FIELD_ANY)
+ vbuf->field = V4L2_FIELD_NONE;
+ if (vbuf->field != V4L2_FIELD_NONE) {
+ dprintk(ctx->dev, "%s field isn't supported\n",
+ __func__);
+ return -EINVAL;
+ }
+ }
+
+ if (vb2_plane_size(vb, 0) < q_data->vb2_sizeimage) {
+ dprintk(ctx->dev,
+ "%s data will not fit into plane (%lu < %lu)\n",
+ __func__, vb2_plane_size(vb, 0),
+ (long)q_data->vb2_sizeimage);
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static void vicodec_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+ unsigned int sz = vb2_get_plane_payload(&vbuf->vb2_buf, 0);
+ u8 *p_src = vb2_plane_vaddr(&vbuf->vb2_buf, 0);
+ u8 *p = p_src;
+ struct vb2_queue *vq_out = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+ V4L2_BUF_TYPE_VIDEO_OUTPUT);
+ struct vb2_queue *vq_cap = v4l2_m2m_get_vq(ctx->fh.m2m_ctx,
+ V4L2_BUF_TYPE_VIDEO_CAPTURE);
+ bool header_valid = false;
+ static const struct v4l2_event rs_event = {
+ .type = V4L2_EVENT_SOURCE_CHANGE,
+ .u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+ };
+
+ if (V4L2_TYPE_IS_CAPTURE(vb->vb2_queue->type) &&
+ vb2_is_streaming(vb->vb2_queue) &&
+ v4l2_m2m_dst_buf_is_last(ctx->fh.m2m_ctx)) {
+ unsigned int i;
+
+ for (i = 0; i < vb->num_planes; i++)
+ vb->planes[i].bytesused = 0;
+
+ vbuf->field = V4L2_FIELD_NONE;
+ vbuf->sequence =
+ get_q_data(ctx, vb->vb2_queue->type)->sequence++;
+
+ v4l2_m2m_last_buffer_done(ctx->fh.m2m_ctx, vbuf);
+ v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
+ return;
+ }
+
+ /* buf_queue handles only the first source change event */
+ if (ctx->first_source_change_sent) {
+ v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+ return;
+ }
+
+ /*
+ * if both queues are streaming, the source change event is
+ * handled in job_ready
+ */
+ if (vb2_is_streaming(vq_cap) && vb2_is_streaming(vq_out)) {
+ v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+ return;
+ }
+
+ /*
+ * source change event is relevant only for the stateful decoder
+ * in the compressed stream
+ */
+ if (ctx->is_stateless || ctx->is_enc ||
+ V4L2_TYPE_IS_CAPTURE(vb->vb2_queue->type)) {
+ v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+ return;
+ }
+
+ do {
+ enum vb2_buffer_state state =
+ get_next_header(ctx, &p, p_src + sz - p);
+
+ if (ctx->header_size < sizeof(struct fwht_cframe_hdr)) {
+ v4l2_m2m_buf_done(vbuf, state);
+ return;
+ }
+ header_valid = is_header_valid(&ctx->state.header);
+ /*
+ * p points right after the end of the header in the
+ * buffer. If the header is invalid we set p to point
+ * to the next byte after the start of the header
+ */
+ if (!header_valid) {
+ p = p - sizeof(struct fwht_cframe_hdr) + 1;
+ if (p < p_src)
+ p = p_src;
+ ctx->header_size = 0;
+ ctx->comp_magic_cnt = 0;
+ }
+
+ } while (!header_valid);
+
+ ctx->cur_buf_offset = p - p_src;
+ update_capture_data_from_header(ctx);
+ ctx->first_source_change_sent = true;
+ v4l2_event_queue_fh(&ctx->fh, &rs_event);
+ v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static void vicodec_return_bufs(struct vb2_queue *q, u32 state)
+{
+ struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
+ struct vb2_v4l2_buffer *vbuf;
+
+ for (;;) {
+ if (V4L2_TYPE_IS_OUTPUT(q->type))
+ vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+ else
+ vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+ if (vbuf == NULL)
+ return;
+ v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req,
+ &ctx->hdl);
+ spin_lock(ctx->lock);
+ v4l2_m2m_buf_done(vbuf, state);
+ spin_unlock(ctx->lock);
+ }
+}
+
+static unsigned int total_frame_size(struct vicodec_q_data *q_data)
+{
+ unsigned int size;
+ unsigned int chroma_div;
+
+ if (!q_data->info) {
+ WARN_ON(1);
+ return 0;
+ }
+ size = q_data->coded_width * q_data->coded_height;
+ chroma_div = q_data->info->width_div * q_data->info->height_div;
+
+ if (q_data->info->components_num == 4)
+ return 2 * size + 2 * (size / chroma_div);
+ else if (q_data->info->components_num == 3)
+ return size + 2 * (size / chroma_div);
+ return size;
+}
+
+static int vicodec_start_streaming(struct vb2_queue *q,
+ unsigned int count)
+{
+ struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
+ struct vicodec_q_data *q_data = get_q_data(ctx, q->type);
+ struct v4l2_fwht_state *state = &ctx->state;
+ const struct v4l2_fwht_pixfmt_info *info = q_data->info;
+ unsigned int size = q_data->coded_width * q_data->coded_height;
+ unsigned int chroma_div;
+ unsigned int total_planes_size;
+ u8 *new_comp_frame = NULL;
+
+ chroma_div = info->width_div * info->height_div;
+ q_data->sequence = 0;
+
+ v4l2_m2m_update_start_streaming_state(ctx->fh.m2m_ctx, q);
+
+ state->gop_cnt = 0;
+
+ if ((V4L2_TYPE_IS_OUTPUT(q->type) && !ctx->is_enc) ||
+ (V4L2_TYPE_IS_CAPTURE(q->type) && ctx->is_enc))
+ return 0;
+
+ if (info->id == V4L2_PIX_FMT_FWHT ||
+ info->id == V4L2_PIX_FMT_FWHT_STATELESS) {
+ vicodec_return_bufs(q, VB2_BUF_STATE_QUEUED);
+ return -EINVAL;
+ }
+ total_planes_size = total_frame_size(q_data);
+ ctx->comp_max_size = total_planes_size;
+
+ state->visible_width = q_data->visible_width;
+ state->visible_height = q_data->visible_height;
+ state->coded_width = q_data->coded_width;
+ state->coded_height = q_data->coded_height;
+ state->stride = q_data->coded_width *
+ info->bytesperline_mult;
+
+ if (ctx->is_stateless) {
+ state->ref_stride = state->stride;
+ return 0;
+ }
+ state->ref_stride = q_data->coded_width * info->luma_alpha_step;
+
+ state->ref_frame.buf = kvmalloc(total_planes_size, GFP_KERNEL);
+ state->ref_frame.luma = state->ref_frame.buf;
+ new_comp_frame = kvmalloc(ctx->comp_max_size, GFP_KERNEL);
+
+ if (!state->ref_frame.luma || !new_comp_frame) {
+ kvfree(state->ref_frame.luma);
+ kvfree(new_comp_frame);
+ vicodec_return_bufs(q, VB2_BUF_STATE_QUEUED);
+ return -ENOMEM;
+ }
+ /*
+ * if state->compressed_frame was already allocated then
+ * it contain data of the first frame of the new resolution
+ */
+ if (state->compressed_frame) {
+ if (ctx->comp_size > ctx->comp_max_size)
+ ctx->comp_size = ctx->comp_max_size;
+
+ memcpy(new_comp_frame,
+ state->compressed_frame, ctx->comp_size);
+ }
+
+ kvfree(state->compressed_frame);
+ state->compressed_frame = new_comp_frame;
+
+ if (info->components_num < 3) {
+ state->ref_frame.cb = NULL;
+ state->ref_frame.cr = NULL;
+ state->ref_frame.alpha = NULL;
+ return 0;
+ }
+
+ state->ref_frame.cb = state->ref_frame.luma + size;
+ state->ref_frame.cr = state->ref_frame.cb + size / chroma_div;
+
+ if (info->components_num == 4)
+ state->ref_frame.alpha =
+ state->ref_frame.cr + size / chroma_div;
+ else
+ state->ref_frame.alpha = NULL;
+
+ return 0;
+}
+
+static void vicodec_stop_streaming(struct vb2_queue *q)
+{
+ struct vicodec_ctx *ctx = vb2_get_drv_priv(q);
+
+ vicodec_return_bufs(q, VB2_BUF_STATE_ERROR);
+
+ v4l2_m2m_update_stop_streaming_state(ctx->fh.m2m_ctx, q);
+
+ if (V4L2_TYPE_IS_OUTPUT(q->type) &&
+ v4l2_m2m_has_stopped(ctx->fh.m2m_ctx))
+ v4l2_event_queue_fh(&ctx->fh, &vicodec_eos_event);
+
+ if (!ctx->is_enc && V4L2_TYPE_IS_OUTPUT(q->type))
+ ctx->first_source_change_sent = false;
+
+ if ((!V4L2_TYPE_IS_OUTPUT(q->type) && !ctx->is_enc) ||
+ (V4L2_TYPE_IS_OUTPUT(q->type) && ctx->is_enc)) {
+ if (!ctx->is_stateless)
+ kvfree(ctx->state.ref_frame.buf);
+ ctx->state.ref_frame.buf = NULL;
+ ctx->state.ref_frame.luma = NULL;
+ ctx->comp_max_size = 0;
+ ctx->source_changed = false;
+ }
+ if (V4L2_TYPE_IS_OUTPUT(q->type) && !ctx->is_enc) {
+ ctx->cur_buf_offset = 0;
+ ctx->comp_size = 0;
+ ctx->header_size = 0;
+ ctx->comp_magic_cnt = 0;
+ ctx->comp_has_frame = false;
+ ctx->comp_has_next_frame = false;
+ }
+}
+
+static void vicodec_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vicodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl);
+}
+
+
+static const struct vb2_ops vicodec_qops = {
+ .queue_setup = vicodec_queue_setup,
+ .buf_out_validate = vicodec_buf_out_validate,
+ .buf_prepare = vicodec_buf_prepare,
+ .buf_queue = vicodec_buf_queue,
+ .buf_request_complete = vicodec_buf_request_complete,
+ .start_streaming = vicodec_start_streaming,
+ .stop_streaming = vicodec_stop_streaming,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+ struct vb2_queue *dst_vq)
+{
+ struct vicodec_ctx *ctx = priv;
+ int ret;
+
+ src_vq->type = (multiplanar ?
+ V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+ V4L2_BUF_TYPE_VIDEO_OUTPUT);
+ src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+ src_vq->drv_priv = ctx;
+ src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+ src_vq->ops = &vicodec_qops;
+ src_vq->mem_ops = &vb2_vmalloc_memops;
+ src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+ if (ctx->is_enc)
+ src_vq->lock = &ctx->dev->stateful_enc.mutex;
+ else if (ctx->is_stateless)
+ src_vq->lock = &ctx->dev->stateless_dec.mutex;
+ else
+ src_vq->lock = &ctx->dev->stateful_dec.mutex;
+ src_vq->supports_requests = ctx->is_stateless;
+ src_vq->requires_requests = ctx->is_stateless;
+ ret = vb2_queue_init(src_vq);
+ if (ret)
+ return ret;
+
+ dst_vq->type = (multiplanar ?
+ V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+ V4L2_BUF_TYPE_VIDEO_CAPTURE);
+ dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+ dst_vq->drv_priv = ctx;
+ dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+ dst_vq->ops = &vicodec_qops;
+ dst_vq->mem_ops = &vb2_vmalloc_memops;
+ dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+ dst_vq->lock = src_vq->lock;
+
+ return vb2_queue_init(dst_vq);
+}
+
+static int vicodec_try_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vicodec_ctx *ctx = container_of(ctrl->handler,
+ struct vicodec_ctx, hdl);
+ const struct v4l2_ctrl_fwht_params *params;
+ struct vicodec_q_data *q_dst = get_q_data(ctx,
+ V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+ switch (ctrl->id) {
+ case V4L2_CID_MPEG_VIDEO_FWHT_PARAMS:
+ if (!q_dst->info)
+ return -EINVAL;
+ params = ctrl->p_new.p_fwht_params;
+ if (params->width > q_dst->coded_width ||
+ params->width < MIN_WIDTH ||
+ params->height > q_dst->coded_height ||
+ params->height < MIN_HEIGHT)
+ return -EINVAL;
+ if (!validate_by_version(params->flags, params->version))
+ return -EINVAL;
+ if (!validate_stateless_params_flags(params, q_dst->info))
+ return -EINVAL;
+ return 0;
+ default:
+ return 0;
+ }
+ return 0;
+}
+
+static void update_header_from_stateless_params(struct vicodec_ctx *ctx,
+ const struct v4l2_ctrl_fwht_params *params)
+{
+ struct fwht_cframe_hdr *p_hdr = &ctx->state.header;
+
+ p_hdr->magic1 = FWHT_MAGIC1;
+ p_hdr->magic2 = FWHT_MAGIC2;
+ p_hdr->version = htonl(params->version);
+ p_hdr->width = htonl(params->width);
+ p_hdr->height = htonl(params->height);
+ p_hdr->flags = htonl(params->flags);
+ p_hdr->colorspace = htonl(params->colorspace);
+ p_hdr->xfer_func = htonl(params->xfer_func);
+ p_hdr->ycbcr_enc = htonl(params->ycbcr_enc);
+ p_hdr->quantization = htonl(params->quantization);
+}
+
+static int vicodec_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vicodec_ctx *ctx = container_of(ctrl->handler,
+ struct vicodec_ctx, hdl);
+ const struct v4l2_ctrl_fwht_params *params;
+
+ switch (ctrl->id) {
+ case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+ ctx->state.gop_size = ctrl->val;
+ return 0;
+ case V4L2_CID_FWHT_I_FRAME_QP:
+ ctx->state.i_frame_qp = ctrl->val;
+ return 0;
+ case V4L2_CID_FWHT_P_FRAME_QP:
+ ctx->state.p_frame_qp = ctrl->val;
+ return 0;
+ case V4L2_CID_MPEG_VIDEO_FWHT_PARAMS:
+ params = ctrl->p_new.p_fwht_params;
+ update_header_from_stateless_params(ctx, params);
+ ctx->state.ref_frame_ts = params->backward_ref_ts;
+ return 0;
+ }
+ return -EINVAL;
+}
+
+static const struct v4l2_ctrl_ops vicodec_ctrl_ops = {
+ .s_ctrl = vicodec_s_ctrl,
+ .try_ctrl = vicodec_try_ctrl,
+};
+
+static const struct v4l2_ctrl_config vicodec_ctrl_stateless_state = {
+ .ops = &vicodec_ctrl_ops,
+ .id = V4L2_CID_MPEG_VIDEO_FWHT_PARAMS,
+ .elem_size = sizeof(struct v4l2_ctrl_fwht_params),
+};
+
+/*
+ * File operations
+ */
+static int vicodec_open(struct file *file)
+{
+ const struct v4l2_fwht_pixfmt_info *info = v4l2_fwht_get_pixfmt(0);
+ struct video_device *vfd = video_devdata(file);
+ struct vicodec_dev *dev = video_drvdata(file);
+ struct vicodec_ctx *ctx = NULL;
+ struct v4l2_ctrl_handler *hdl;
+ unsigned int raw_size;
+ unsigned int comp_size;
+ int rc = 0;
+
+ if (mutex_lock_interruptible(vfd->lock))
+ return -ERESTARTSYS;
+ ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+ if (!ctx) {
+ rc = -ENOMEM;
+ goto open_unlock;
+ }
+
+ if (vfd == &dev->stateful_enc.vfd)
+ ctx->is_enc = true;
+ else if (vfd == &dev->stateless_dec.vfd)
+ ctx->is_stateless = true;
+
+ v4l2_fh_init(&ctx->fh, video_devdata(file));
+ file->private_data = &ctx->fh;
+ ctx->dev = dev;
+ hdl = &ctx->hdl;
+ v4l2_ctrl_handler_init(hdl, 5);
+ v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
+ 1, 16, 1, 10);
+ v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops, V4L2_CID_FWHT_I_FRAME_QP,
+ 1, 31, 1, 20);
+ v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops, V4L2_CID_FWHT_P_FRAME_QP,
+ 1, 31, 1, 20);
+ if (ctx->is_enc)
+ v4l2_ctrl_new_std(hdl, &vicodec_ctrl_ops,
+ V4L2_CID_MIN_BUFFERS_FOR_OUTPUT, 1, 1, 1, 1);
+ if (ctx->is_stateless)
+ v4l2_ctrl_new_custom(hdl, &vicodec_ctrl_stateless_state, NULL);
+ if (hdl->error) {
+ rc = hdl->error;
+ v4l2_ctrl_handler_free(hdl);
+ kfree(ctx);
+ goto open_unlock;
+ }
+ ctx->fh.ctrl_handler = hdl;
+ v4l2_ctrl_handler_setup(hdl);
+
+ if (ctx->is_enc)
+ ctx->q_data[V4L2_M2M_SRC].info = info;
+ else if (ctx->is_stateless)
+ ctx->q_data[V4L2_M2M_SRC].info = &pixfmt_stateless_fwht;
+ else
+ ctx->q_data[V4L2_M2M_SRC].info = &pixfmt_fwht;
+ ctx->q_data[V4L2_M2M_SRC].coded_width = 1280;
+ ctx->q_data[V4L2_M2M_SRC].coded_height = 720;
+ ctx->q_data[V4L2_M2M_SRC].visible_width = 1280;
+ ctx->q_data[V4L2_M2M_SRC].visible_height = 720;
+ raw_size = 1280 * 720 * info->sizeimage_mult / info->sizeimage_div;
+ comp_size = 1280 * 720 * pixfmt_fwht.sizeimage_mult /
+ pixfmt_fwht.sizeimage_div;
+ if (ctx->is_enc)
+ ctx->q_data[V4L2_M2M_SRC].sizeimage = raw_size;
+ else if (ctx->is_stateless)
+ ctx->q_data[V4L2_M2M_SRC].sizeimage = comp_size;
+ else
+ ctx->q_data[V4L2_M2M_SRC].sizeimage =
+ comp_size + sizeof(struct fwht_cframe_hdr);
+ ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
+ if (ctx->is_enc) {
+ ctx->q_data[V4L2_M2M_DST].info = &pixfmt_fwht;
+ ctx->q_data[V4L2_M2M_DST].sizeimage =
+ comp_size + sizeof(struct fwht_cframe_hdr);
+ } else {
+ ctx->q_data[V4L2_M2M_DST].info = info;
+ ctx->q_data[V4L2_M2M_DST].sizeimage = raw_size;
+ }
+
+ ctx->state.colorspace = V4L2_COLORSPACE_REC709;
+
+ if (ctx->is_enc) {
+ ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->stateful_enc.m2m_dev,
+ ctx, &queue_init);
+ ctx->lock = &dev->stateful_enc.lock;
+ } else if (ctx->is_stateless) {
+ ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->stateless_dec.m2m_dev,
+ ctx, &queue_init);
+ ctx->lock = &dev->stateless_dec.lock;
+ } else {
+ ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->stateful_dec.m2m_dev,
+ ctx, &queue_init);
+ ctx->lock = &dev->stateful_dec.lock;
+ }
+
+ if (IS_ERR(ctx->fh.m2m_ctx)) {
+ rc = PTR_ERR(ctx->fh.m2m_ctx);
+
+ v4l2_ctrl_handler_free(hdl);
+ v4l2_fh_exit(&ctx->fh);
+ kfree(ctx);
+ goto open_unlock;
+ }
+
+ v4l2_fh_add(&ctx->fh);
+
+open_unlock:
+ mutex_unlock(vfd->lock);
+ return rc;
+}
+
+static int vicodec_release(struct file *file)
+{
+ struct video_device *vfd = video_devdata(file);
+ struct vicodec_ctx *ctx = file2ctx(file);
+
+ mutex_lock(vfd->lock);
+ v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+ mutex_unlock(vfd->lock);
+ v4l2_fh_del(&ctx->fh);
+ v4l2_fh_exit(&ctx->fh);
+ v4l2_ctrl_handler_free(&ctx->hdl);
+ kvfree(ctx->state.compressed_frame);
+ kfree(ctx);
+
+ return 0;
+}
+
+static int vicodec_request_validate(struct media_request *req)
+{
+ struct media_request_object *obj;
+ struct v4l2_ctrl_handler *parent_hdl, *hdl;
+ struct vicodec_ctx *ctx = NULL;
+ struct v4l2_ctrl *ctrl;
+ unsigned int count;
+
+ list_for_each_entry(obj, &req->objects, list) {
+ struct vb2_buffer *vb;
+
+ if (vb2_request_object_is_buffer(obj)) {
+ vb = container_of(obj, struct vb2_buffer, req_obj);
+ ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+ break;
+ }
+ }
+
+ if (!ctx) {
+ pr_err("No buffer was provided with the request\n");
+ return -ENOENT;
+ }
+
+ count = vb2_request_buffer_cnt(req);
+ if (!count) {
+ v4l2_info(&ctx->dev->v4l2_dev,
+ "No buffer was provided with the request\n");
+ return -ENOENT;
+ } else if (count > 1) {
+ v4l2_info(&ctx->dev->v4l2_dev,
+ "More than one buffer was provided with the request\n");
+ return -EINVAL;
+ }
+
+ parent_hdl = &ctx->hdl;
+
+ hdl = v4l2_ctrl_request_hdl_find(req, parent_hdl);
+ if (!hdl) {
+ v4l2_info(&ctx->dev->v4l2_dev, "Missing codec control\n");
+ return -ENOENT;
+ }
+ ctrl = v4l2_ctrl_request_hdl_ctrl_find(hdl,
+ vicodec_ctrl_stateless_state.id);
+ v4l2_ctrl_request_hdl_put(hdl);
+ if (!ctrl) {
+ v4l2_info(&ctx->dev->v4l2_dev,
+ "Missing required codec control\n");
+ return -ENOENT;
+ }
+
+ return vb2_request_validate(req);
+}
+
+static const struct v4l2_file_operations vicodec_fops = {
+ .owner = THIS_MODULE,
+ .open = vicodec_open,
+ .release = vicodec_release,
+ .poll = v4l2_m2m_fop_poll,
+ .unlocked_ioctl = video_ioctl2,
+ .mmap = v4l2_m2m_fop_mmap,
+};
+
+static const struct video_device vicodec_videodev = {
+ .name = VICODEC_NAME,
+ .vfl_dir = VFL_DIR_M2M,
+ .fops = &vicodec_fops,
+ .ioctl_ops = &vicodec_ioctl_ops,
+ .minor = -1,
+ .release = video_device_release_empty,
+};
+
+static const struct media_device_ops vicodec_m2m_media_ops = {
+ .req_validate = vicodec_request_validate,
+ .req_queue = v4l2_m2m_request_queue,
+};
+
+static const struct v4l2_m2m_ops m2m_ops = {
+ .device_run = device_run,
+ .job_ready = job_ready,
+};
+
+static int register_instance(struct vicodec_dev *dev,
+ struct vicodec_dev_instance *dev_instance,
+ const char *name, bool is_enc)
+{
+ struct video_device *vfd;
+ int ret;
+
+ spin_lock_init(&dev_instance->lock);
+ mutex_init(&dev_instance->mutex);
+ dev_instance->m2m_dev = v4l2_m2m_init(&m2m_ops);
+ if (IS_ERR(dev_instance->m2m_dev)) {
+ v4l2_err(&dev->v4l2_dev, "Failed to init vicodec enc device\n");
+ return PTR_ERR(dev_instance->m2m_dev);
+ }
+
+ dev_instance->vfd = vicodec_videodev;
+ vfd = &dev_instance->vfd;
+ vfd->lock = &dev_instance->mutex;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ strscpy(vfd->name, name, sizeof(vfd->name));
+ vfd->device_caps = V4L2_CAP_STREAMING |
+ (multiplanar ? V4L2_CAP_VIDEO_M2M_MPLANE : V4L2_CAP_VIDEO_M2M);
+ if (is_enc) {
+ v4l2_disable_ioctl(vfd, VIDIOC_DECODER_CMD);
+ v4l2_disable_ioctl(vfd, VIDIOC_TRY_DECODER_CMD);
+ } else {
+ v4l2_disable_ioctl(vfd, VIDIOC_ENCODER_CMD);
+ v4l2_disable_ioctl(vfd, VIDIOC_TRY_ENCODER_CMD);
+ }
+ video_set_drvdata(vfd, dev);
+
+ ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
+ if (ret) {
+ v4l2_err(&dev->v4l2_dev, "Failed to register video device '%s'\n", name);
+ v4l2_m2m_release(dev_instance->m2m_dev);
+ return ret;
+ }
+ v4l2_info(&dev->v4l2_dev, "Device '%s' registered as /dev/video%d\n",
+ name, vfd->num);
+ return 0;
+}
+
+static void vicodec_v4l2_dev_release(struct v4l2_device *v4l2_dev)
+{
+ struct vicodec_dev *dev = container_of(v4l2_dev, struct vicodec_dev, v4l2_dev);
+
+ v4l2_device_unregister(&dev->v4l2_dev);
+ v4l2_m2m_release(dev->stateful_enc.m2m_dev);
+ v4l2_m2m_release(dev->stateful_dec.m2m_dev);
+ v4l2_m2m_release(dev->stateless_dec.m2m_dev);
+#ifdef CONFIG_MEDIA_CONTROLLER
+ media_device_cleanup(&dev->mdev);
+#endif
+ kfree(dev);
+}
+
+static int vicodec_probe(struct platform_device *pdev)
+{
+ struct vicodec_dev *dev;
+ int ret;
+
+ dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+ if (!dev)
+ return -ENOMEM;
+
+ ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+ if (ret)
+ goto free_dev;
+
+ dev->v4l2_dev.release = vicodec_v4l2_dev_release;
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->mdev.dev = &pdev->dev;
+ strscpy(dev->mdev.model, "vicodec", sizeof(dev->mdev.model));
+ strscpy(dev->mdev.bus_info, "platform:vicodec",
+ sizeof(dev->mdev.bus_info));
+ media_device_init(&dev->mdev);
+ dev->mdev.ops = &vicodec_m2m_media_ops;
+ dev->v4l2_dev.mdev = &dev->mdev;
+#endif
+
+ platform_set_drvdata(pdev, dev);
+
+ ret = register_instance(dev, &dev->stateful_enc, "stateful-encoder",
+ true);
+ if (ret)
+ goto unreg_dev;
+
+ ret = register_instance(dev, &dev->stateful_dec, "stateful-decoder",
+ false);
+ if (ret)
+ goto unreg_sf_enc;
+
+ ret = register_instance(dev, &dev->stateless_dec, "stateless-decoder",
+ false);
+ if (ret)
+ goto unreg_sf_dec;
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ ret = v4l2_m2m_register_media_controller(dev->stateful_enc.m2m_dev,
+ &dev->stateful_enc.vfd,
+ MEDIA_ENT_F_PROC_VIDEO_ENCODER);
+ if (ret) {
+ v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller for enc\n");
+ goto unreg_m2m;
+ }
+
+ ret = v4l2_m2m_register_media_controller(dev->stateful_dec.m2m_dev,
+ &dev->stateful_dec.vfd,
+ MEDIA_ENT_F_PROC_VIDEO_DECODER);
+ if (ret) {
+ v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller for dec\n");
+ goto unreg_m2m_sf_enc_mc;
+ }
+
+ ret = v4l2_m2m_register_media_controller(dev->stateless_dec.m2m_dev,
+ &dev->stateless_dec.vfd,
+ MEDIA_ENT_F_PROC_VIDEO_DECODER);
+ if (ret) {
+ v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller for stateless dec\n");
+ goto unreg_m2m_sf_dec_mc;
+ }
+
+ ret = media_device_register(&dev->mdev);
+ if (ret) {
+ v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n");
+ goto unreg_m2m_sl_dec_mc;
+ }
+#endif
+ return 0;
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+unreg_m2m_sl_dec_mc:
+ v4l2_m2m_unregister_media_controller(dev->stateless_dec.m2m_dev);
+unreg_m2m_sf_dec_mc:
+ v4l2_m2m_unregister_media_controller(dev->stateful_dec.m2m_dev);
+unreg_m2m_sf_enc_mc:
+ v4l2_m2m_unregister_media_controller(dev->stateful_enc.m2m_dev);
+unreg_m2m:
+ video_unregister_device(&dev->stateless_dec.vfd);
+ v4l2_m2m_release(dev->stateless_dec.m2m_dev);
+#endif
+unreg_sf_dec:
+ video_unregister_device(&dev->stateful_dec.vfd);
+ v4l2_m2m_release(dev->stateful_dec.m2m_dev);
+unreg_sf_enc:
+ video_unregister_device(&dev->stateful_enc.vfd);
+ v4l2_m2m_release(dev->stateful_enc.m2m_dev);
+unreg_dev:
+ v4l2_device_unregister(&dev->v4l2_dev);
+free_dev:
+ kfree(dev);
+
+ return ret;
+}
+
+static int vicodec_remove(struct platform_device *pdev)
+{
+ struct vicodec_dev *dev = platform_get_drvdata(pdev);
+
+ v4l2_info(&dev->v4l2_dev, "Removing " VICODEC_NAME);
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ media_device_unregister(&dev->mdev);
+ v4l2_m2m_unregister_media_controller(dev->stateful_enc.m2m_dev);
+ v4l2_m2m_unregister_media_controller(dev->stateful_dec.m2m_dev);
+ v4l2_m2m_unregister_media_controller(dev->stateless_dec.m2m_dev);
+#endif
+
+ video_unregister_device(&dev->stateful_enc.vfd);
+ video_unregister_device(&dev->stateful_dec.vfd);
+ video_unregister_device(&dev->stateless_dec.vfd);
+ v4l2_device_put(&dev->v4l2_dev);
+
+ return 0;
+}
+
+static struct platform_driver vicodec_pdrv = {
+ .probe = vicodec_probe,
+ .remove = vicodec_remove,
+ .driver = {
+ .name = VICODEC_NAME,
+ },
+};
+
+static void __exit vicodec_exit(void)
+{
+ platform_driver_unregister(&vicodec_pdrv);
+ platform_device_unregister(&vicodec_pdev);
+}
+
+static int __init vicodec_init(void)
+{
+ int ret;
+
+ ret = platform_device_register(&vicodec_pdev);
+ if (ret)
+ return ret;
+
+ ret = platform_driver_register(&vicodec_pdrv);
+ if (ret)
+ platform_device_unregister(&vicodec_pdev);
+
+ return ret;
+}
+
+module_init(vicodec_init);
+module_exit(vicodec_exit);
diff --git a/drivers/media/test-drivers/vidtv/Kconfig b/drivers/media/test-drivers/vidtv/Kconfig
new file mode 100644
index 0000000..22c4fd3
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/Kconfig
@@ -0,0 +1,11 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config DVB_VIDTV
+ tristate "Virtual DVB Driver (vidtv)"
+ depends on DVB_CORE && MEDIA_SUPPORT && I2C
+ help
+ The virtual DVB test driver 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.
+
+
+ When in doubt, say N.
diff --git a/drivers/media/test-drivers/vidtv/Makefile b/drivers/media/test-drivers/vidtv/Makefile
new file mode 100644
index 0000000..330089e
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/Makefile
@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+
+dvb-vidtv-tuner-objs := vidtv_tuner.o
+dvb-vidtv-demod-objs := vidtv_demod.o
+dvb-vidtv-bridge-objs := vidtv_bridge.o vidtv_common.o vidtv_ts.o vidtv_psi.o \
+ vidtv_pes.o vidtv_s302m.o vidtv_channel.o vidtv_mux.o
+
+obj-$(CONFIG_DVB_VIDTV) += dvb-vidtv-tuner.o dvb-vidtv-demod.o \
+ dvb-vidtv-bridge.o
diff --git a/drivers/media/test-drivers/vidtv/vidtv_bridge.c b/drivers/media/test-drivers/vidtv/vidtv_bridge.c
new file mode 100644
index 0000000..fc64d0c
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_bridge.c
@@ -0,0 +1,580 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * The Virtual DTV test driver 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.
+ *
+ * When this module is loaded, it will attempt to modprobe 'dvb_vidtv_tuner'
+ * and 'dvb_vidtv_demod'.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#include <linux/dev_printk.h>
+#include <linux/moduleparam.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/time.h>
+#include <linux/types.h>
+#include <linux/workqueue.h>
+
+#include "vidtv_bridge.h"
+#include "vidtv_common.h"
+#include "vidtv_demod.h"
+#include "vidtv_mux.h"
+#include "vidtv_ts.h"
+#include "vidtv_tuner.h"
+
+#define MUX_BUF_MIN_SZ 90164
+#define MUX_BUF_MAX_SZ (MUX_BUF_MIN_SZ * 10)
+#define TUNER_DEFAULT_ADDR 0x68
+#define DEMOD_DEFAULT_ADDR 0x60
+#define VIDTV_DEFAULT_NETWORK_ID 0xff44
+#define VIDTV_DEFAULT_NETWORK_NAME "LinuxTV.org"
+#define VIDTV_DEFAULT_TS_ID 0x4081
+
+/*
+ * The LNBf fake parameters here are the ranges used by an
+ * Universal (extended) European LNBf, which is likely the most common LNBf
+ * found on Satellite digital TV system nowadays.
+ */
+#define LNB_CUT_FREQUENCY 11700000 /* high IF frequency */
+#define LNB_LOW_FREQ 9750000 /* low IF frequency */
+#define LNB_HIGH_FREQ 10600000 /* transition frequency */
+
+static unsigned int drop_tslock_prob_on_low_snr;
+module_param(drop_tslock_prob_on_low_snr, uint, 0);
+MODULE_PARM_DESC(drop_tslock_prob_on_low_snr,
+ "Probability of losing the TS lock if the signal quality is bad");
+
+static unsigned int recover_tslock_prob_on_good_snr;
+module_param(recover_tslock_prob_on_good_snr, uint, 0);
+MODULE_PARM_DESC(recover_tslock_prob_on_good_snr,
+ "Probability recovering the TS lock when the signal improves");
+
+static unsigned int mock_power_up_delay_msec;
+module_param(mock_power_up_delay_msec, uint, 0);
+MODULE_PARM_DESC(mock_power_up_delay_msec, "Simulate a power up delay");
+
+static unsigned int mock_tune_delay_msec;
+module_param(mock_tune_delay_msec, uint, 0);
+MODULE_PARM_DESC(mock_tune_delay_msec, "Simulate a tune delay");
+
+static unsigned int vidtv_valid_dvb_t_freqs[NUM_VALID_TUNER_FREQS] = {
+ 474000000
+};
+
+module_param_array(vidtv_valid_dvb_t_freqs, uint, NULL, 0);
+MODULE_PARM_DESC(vidtv_valid_dvb_t_freqs,
+ "Valid DVB-T frequencies to simulate, in Hz");
+
+static unsigned int vidtv_valid_dvb_c_freqs[NUM_VALID_TUNER_FREQS] = {
+ 474000000
+};
+
+module_param_array(vidtv_valid_dvb_c_freqs, uint, NULL, 0);
+MODULE_PARM_DESC(vidtv_valid_dvb_c_freqs,
+ "Valid DVB-C frequencies to simulate, in Hz");
+
+static unsigned int vidtv_valid_dvb_s_freqs[NUM_VALID_TUNER_FREQS] = {
+ 11362000
+};
+module_param_array(vidtv_valid_dvb_s_freqs, uint, NULL, 0);
+MODULE_PARM_DESC(vidtv_valid_dvb_s_freqs,
+ "Valid DVB-S/S2 frequencies to simulate at Ku-Band, in kHz");
+
+static unsigned int max_frequency_shift_hz;
+module_param(max_frequency_shift_hz, uint, 0);
+MODULE_PARM_DESC(max_frequency_shift_hz,
+ "Maximum shift in HZ allowed when tuning in a channel");
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nums);
+
+/*
+ * Influences the signal acquisition time. See ISO/IEC 13818-1 : 2000. p. 113.
+ */
+static unsigned int si_period_msec = 40;
+module_param(si_period_msec, uint, 0);
+MODULE_PARM_DESC(si_period_msec, "How often to send SI packets. Default: 40ms");
+
+static unsigned int pcr_period_msec = 40;
+module_param(pcr_period_msec, uint, 0);
+MODULE_PARM_DESC(pcr_period_msec,
+ "How often to send PCR packets. Default: 40ms");
+
+static unsigned int mux_rate_kbytes_sec = 4096;
+module_param(mux_rate_kbytes_sec, uint, 0);
+MODULE_PARM_DESC(mux_rate_kbytes_sec, "Mux rate: will pad stream if below");
+
+static unsigned int pcr_pid = 0x200;
+module_param(pcr_pid, uint, 0);
+MODULE_PARM_DESC(pcr_pid, "PCR PID for all channels: defaults to 0x200");
+
+static unsigned int mux_buf_sz_pkts;
+module_param(mux_buf_sz_pkts, uint, 0);
+MODULE_PARM_DESC(mux_buf_sz_pkts,
+ "Size for the internal mux buffer in multiples of 188 bytes");
+
+static u32 vidtv_bridge_mux_buf_sz_for_mux_rate(void)
+{
+ u32 max_elapsed_time_msecs = VIDTV_MAX_SLEEP_USECS / USEC_PER_MSEC;
+ u32 mux_buf_sz = mux_buf_sz_pkts * TS_PACKET_LEN;
+ u32 nbytes_expected;
+
+ nbytes_expected = mux_rate_kbytes_sec;
+ nbytes_expected *= max_elapsed_time_msecs;
+
+ mux_buf_sz = roundup(nbytes_expected, TS_PACKET_LEN);
+ mux_buf_sz += mux_buf_sz / 10;
+
+ if (mux_buf_sz < MUX_BUF_MIN_SZ)
+ mux_buf_sz = MUX_BUF_MIN_SZ;
+
+ if (mux_buf_sz > MUX_BUF_MAX_SZ)
+ mux_buf_sz = MUX_BUF_MAX_SZ;
+
+ return mux_buf_sz;
+}
+
+static bool vidtv_bridge_check_demod_lock(struct vidtv_dvb *dvb, u32 n)
+{
+ enum fe_status status;
+
+ dvb->fe[n]->ops.read_status(dvb->fe[n], &status);
+
+ return status == (FE_HAS_SIGNAL |
+ FE_HAS_CARRIER |
+ FE_HAS_VITERBI |
+ FE_HAS_SYNC |
+ FE_HAS_LOCK);
+}
+
+/*
+ * called on a separate thread by the mux when new packets become available
+ */
+static void vidtv_bridge_on_new_pkts_avail(void *priv, u8 *buf, u32 npkts)
+{
+ struct vidtv_dvb *dvb = priv;
+
+ /* drop packets if we lose the lock */
+ if (vidtv_bridge_check_demod_lock(dvb, 0))
+ dvb_dmx_swfilter_packets(&dvb->demux, buf, npkts);
+}
+
+static int vidtv_start_streaming(struct vidtv_dvb *dvb)
+{
+ struct vidtv_mux_init_args mux_args = {
+ .mux_rate_kbytes_sec = mux_rate_kbytes_sec,
+ .on_new_packets_available_cb = vidtv_bridge_on_new_pkts_avail,
+ .pcr_period_usecs = pcr_period_msec * USEC_PER_MSEC,
+ .si_period_usecs = si_period_msec * USEC_PER_MSEC,
+ .pcr_pid = pcr_pid,
+ .transport_stream_id = VIDTV_DEFAULT_TS_ID,
+ .network_id = VIDTV_DEFAULT_NETWORK_ID,
+ .network_name = VIDTV_DEFAULT_NETWORK_NAME,
+ .priv = dvb,
+ };
+ struct device *dev = &dvb->pdev->dev;
+ u32 mux_buf_sz;
+
+ if (dvb->streaming) {
+ dev_warn_ratelimited(dev, "Already streaming. Skipping.\n");
+ return 0;
+ }
+
+ if (mux_buf_sz_pkts)
+ mux_buf_sz = mux_buf_sz_pkts;
+ else
+ mux_buf_sz = vidtv_bridge_mux_buf_sz_for_mux_rate();
+
+ mux_args.mux_buf_sz = mux_buf_sz;
+
+ dvb->streaming = true;
+ dvb->mux = vidtv_mux_init(dvb->fe[0], dev, &mux_args);
+ if (!dvb->mux)
+ return -ENOMEM;
+ vidtv_mux_start_thread(dvb->mux);
+
+ dev_dbg_ratelimited(dev, "Started streaming\n");
+ return 0;
+}
+
+static int vidtv_stop_streaming(struct vidtv_dvb *dvb)
+{
+ struct device *dev = &dvb->pdev->dev;
+
+ dvb->streaming = false;
+ vidtv_mux_stop_thread(dvb->mux);
+ vidtv_mux_destroy(dvb->mux);
+ dvb->mux = NULL;
+
+ dev_dbg_ratelimited(dev, "Stopped streaming\n");
+ return 0;
+}
+
+static int vidtv_start_feed(struct dvb_demux_feed *feed)
+{
+ struct dvb_demux *demux = feed->demux;
+ struct vidtv_dvb *dvb = demux->priv;
+ int ret;
+ int rc;
+
+ if (!demux->dmx.frontend)
+ return -EINVAL;
+
+ mutex_lock(&dvb->feed_lock);
+
+ dvb->nfeeds++;
+ rc = dvb->nfeeds;
+
+ if (dvb->nfeeds == 1) {
+ ret = vidtv_start_streaming(dvb);
+ if (ret < 0)
+ rc = ret;
+ }
+
+ mutex_unlock(&dvb->feed_lock);
+ return rc;
+}
+
+static int vidtv_stop_feed(struct dvb_demux_feed *feed)
+{
+ struct dvb_demux *demux = feed->demux;
+ struct vidtv_dvb *dvb = demux->priv;
+ int err = 0;
+
+ mutex_lock(&dvb->feed_lock);
+ dvb->nfeeds--;
+
+ if (!dvb->nfeeds)
+ err = vidtv_stop_streaming(dvb);
+
+ mutex_unlock(&dvb->feed_lock);
+ return err;
+}
+
+static struct dvb_frontend *vidtv_get_frontend_ptr(struct i2c_client *c)
+{
+ struct vidtv_demod_state *state = i2c_get_clientdata(c);
+
+ /* the demod will set this when its probe function runs */
+ return &state->frontend;
+}
+
+static int vidtv_master_xfer(struct i2c_adapter *i2c_adap,
+ struct i2c_msg msgs[],
+ int num)
+{
+ /*
+ * Right now, this virtual driver doesn't really send or receive
+ * messages from I2C. A real driver will require an implementation
+ * here.
+ */
+ return 0;
+}
+
+static u32 vidtv_i2c_func(struct i2c_adapter *adapter)
+{
+ return I2C_FUNC_I2C;
+}
+
+static const struct i2c_algorithm vidtv_i2c_algorithm = {
+ .master_xfer = vidtv_master_xfer,
+ .functionality = vidtv_i2c_func,
+};
+
+static int vidtv_bridge_i2c_register_adap(struct vidtv_dvb *dvb)
+{
+ struct i2c_adapter *i2c_adapter = &dvb->i2c_adapter;
+
+ strscpy(i2c_adapter->name, "vidtv_i2c", sizeof(i2c_adapter->name));
+ i2c_adapter->owner = THIS_MODULE;
+ i2c_adapter->algo = &vidtv_i2c_algorithm;
+ i2c_adapter->algo_data = NULL;
+ i2c_adapter->timeout = 500;
+ i2c_adapter->retries = 3;
+ i2c_adapter->dev.parent = &dvb->pdev->dev;
+
+ i2c_set_adapdata(i2c_adapter, dvb);
+ return i2c_add_adapter(&dvb->i2c_adapter);
+}
+
+static int vidtv_bridge_register_adap(struct vidtv_dvb *dvb)
+{
+ int ret = 0;
+
+ ret = dvb_register_adapter(&dvb->adapter,
+ KBUILD_MODNAME,
+ THIS_MODULE,
+ &dvb->i2c_adapter.dev,
+ adapter_nums);
+
+ return ret;
+}
+
+static int vidtv_bridge_dmx_init(struct vidtv_dvb *dvb)
+{
+ dvb->demux.dmx.capabilities = DMX_TS_FILTERING |
+ DMX_SECTION_FILTERING;
+
+ dvb->demux.priv = dvb;
+ dvb->demux.filternum = 256;
+ dvb->demux.feednum = 256;
+ dvb->demux.start_feed = vidtv_start_feed;
+ dvb->demux.stop_feed = vidtv_stop_feed;
+
+ return dvb_dmx_init(&dvb->demux);
+}
+
+static int vidtv_bridge_dmxdev_init(struct vidtv_dvb *dvb)
+{
+ dvb->dmx_dev.filternum = 256;
+ dvb->dmx_dev.demux = &dvb->demux.dmx;
+ dvb->dmx_dev.capabilities = 0;
+
+ return dvb_dmxdev_init(&dvb->dmx_dev, &dvb->adapter);
+}
+
+static int vidtv_bridge_probe_demod(struct vidtv_dvb *dvb, u32 n)
+{
+ struct vidtv_demod_config cfg = {
+ .drop_tslock_prob_on_low_snr = drop_tslock_prob_on_low_snr,
+ .recover_tslock_prob_on_good_snr = recover_tslock_prob_on_good_snr,
+ };
+ dvb->i2c_client_demod[n] = dvb_module_probe("dvb_vidtv_demod",
+ NULL,
+ &dvb->i2c_adapter,
+ DEMOD_DEFAULT_ADDR,
+ &cfg);
+
+ /* driver will not work anyways so bail out */
+ if (!dvb->i2c_client_demod[n])
+ return -ENODEV;
+
+ /* retrieve a ptr to the frontend state */
+ dvb->fe[n] = vidtv_get_frontend_ptr(dvb->i2c_client_demod[n]);
+
+ return 0;
+}
+
+static int vidtv_bridge_probe_tuner(struct vidtv_dvb *dvb, u32 n)
+{
+ struct vidtv_tuner_config cfg = {
+ .fe = dvb->fe[n],
+ .mock_power_up_delay_msec = mock_power_up_delay_msec,
+ .mock_tune_delay_msec = mock_tune_delay_msec,
+ };
+ u32 freq;
+ int i;
+
+ /* TODO: check if the frequencies are at a valid range */
+
+ memcpy(cfg.vidtv_valid_dvb_t_freqs,
+ vidtv_valid_dvb_t_freqs,
+ sizeof(vidtv_valid_dvb_t_freqs));
+
+ memcpy(cfg.vidtv_valid_dvb_c_freqs,
+ vidtv_valid_dvb_c_freqs,
+ sizeof(vidtv_valid_dvb_c_freqs));
+
+ /*
+ * Convert Satellite frequencies from Ku-band in kHZ into S-band
+ * frequencies in Hz.
+ */
+ for (i = 0; i < ARRAY_SIZE(vidtv_valid_dvb_s_freqs); i++) {
+ freq = vidtv_valid_dvb_s_freqs[i];
+ if (freq) {
+ if (freq < LNB_CUT_FREQUENCY)
+ freq = abs(freq - LNB_LOW_FREQ);
+ else
+ freq = abs(freq - LNB_HIGH_FREQ);
+ }
+ cfg.vidtv_valid_dvb_s_freqs[i] = freq;
+ }
+
+ cfg.max_frequency_shift_hz = max_frequency_shift_hz;
+
+ dvb->i2c_client_tuner[n] = dvb_module_probe("dvb_vidtv_tuner",
+ NULL,
+ &dvb->i2c_adapter,
+ TUNER_DEFAULT_ADDR,
+ &cfg);
+
+ return (dvb->i2c_client_tuner[n]) ? 0 : -ENODEV;
+}
+
+static int vidtv_bridge_dvb_init(struct vidtv_dvb *dvb)
+{
+ int ret, i, j;
+
+ ret = vidtv_bridge_i2c_register_adap(dvb);
+ if (ret < 0)
+ goto fail_i2c;
+
+ ret = vidtv_bridge_register_adap(dvb);
+ if (ret < 0)
+ goto fail_adapter;
+
+ for (i = 0; i < NUM_FE; ++i) {
+ ret = vidtv_bridge_probe_demod(dvb, i);
+ if (ret < 0)
+ goto fail_demod_probe;
+
+ ret = vidtv_bridge_probe_tuner(dvb, i);
+ if (ret < 0)
+ goto fail_tuner_probe;
+
+ ret = dvb_register_frontend(&dvb->adapter, dvb->fe[i]);
+ if (ret < 0)
+ goto fail_fe;
+ }
+
+ ret = vidtv_bridge_dmx_init(dvb);
+ if (ret < 0)
+ goto fail_dmx;
+
+ ret = vidtv_bridge_dmxdev_init(dvb);
+ if (ret < 0)
+ goto fail_dmx_dev;
+
+ for (j = 0; j < NUM_FE; ++j) {
+ ret = dvb->demux.dmx.connect_frontend(&dvb->demux.dmx,
+ &dvb->dmx_fe[j]);
+ if (ret < 0)
+ goto fail_dmx_conn;
+
+ /*
+ * The source of the demux is a frontend connected
+ * to the demux.
+ */
+ dvb->dmx_fe[j].source = DMX_FRONTEND_0;
+ }
+
+ return ret;
+
+fail_dmx_conn:
+ for (j = j - 1; j >= 0; --j)
+ dvb->demux.dmx.remove_frontend(&dvb->demux.dmx,
+ &dvb->dmx_fe[j]);
+fail_dmx_dev:
+ dvb_dmxdev_release(&dvb->dmx_dev);
+fail_dmx:
+ dvb_dmx_release(&dvb->demux);
+fail_fe:
+ for (j = i; j >= 0; --j)
+ dvb_unregister_frontend(dvb->fe[j]);
+fail_tuner_probe:
+ for (j = i; j >= 0; --j)
+ if (dvb->i2c_client_tuner[j])
+ dvb_module_release(dvb->i2c_client_tuner[j]);
+
+fail_demod_probe:
+ for (j = i; j >= 0; --j)
+ if (dvb->i2c_client_demod[j])
+ dvb_module_release(dvb->i2c_client_demod[j]);
+
+fail_adapter:
+ dvb_unregister_adapter(&dvb->adapter);
+
+fail_i2c:
+ i2c_del_adapter(&dvb->i2c_adapter);
+
+ return ret;
+}
+
+static int vidtv_bridge_probe(struct platform_device *pdev)
+{
+ struct vidtv_dvb *dvb;
+ int ret;
+
+ dvb = kzalloc(sizeof(*dvb), GFP_KERNEL);
+ if (!dvb)
+ return -ENOMEM;
+
+ dvb->pdev = pdev;
+
+ ret = vidtv_bridge_dvb_init(dvb);
+ if (ret < 0)
+ goto err_dvb;
+
+ mutex_init(&dvb->feed_lock);
+
+ platform_set_drvdata(pdev, dvb);
+
+ dev_info(&pdev->dev, "Successfully initialized vidtv!\n");
+ return ret;
+
+err_dvb:
+ kfree(dvb);
+ return ret;
+}
+
+static int vidtv_bridge_remove(struct platform_device *pdev)
+{
+ struct vidtv_dvb *dvb;
+ u32 i;
+
+ dvb = platform_get_drvdata(pdev);
+
+ mutex_destroy(&dvb->feed_lock);
+
+ for (i = 0; i < NUM_FE; ++i) {
+ dvb_unregister_frontend(dvb->fe[i]);
+ dvb_module_release(dvb->i2c_client_tuner[i]);
+ dvb_module_release(dvb->i2c_client_demod[i]);
+ }
+
+ dvb_dmxdev_release(&dvb->dmx_dev);
+ dvb_dmx_release(&dvb->demux);
+ dvb_unregister_adapter(&dvb->adapter);
+
+ return 0;
+}
+
+static void vidtv_bridge_dev_release(struct device *dev)
+{
+}
+
+static struct platform_device vidtv_bridge_dev = {
+ .name = "vidtv_bridge",
+ .dev.release = vidtv_bridge_dev_release,
+};
+
+static struct platform_driver vidtv_bridge_driver = {
+ .driver = {
+ .name = "vidtv_bridge",
+ .suppress_bind_attrs = true,
+ },
+ .probe = vidtv_bridge_probe,
+ .remove = vidtv_bridge_remove,
+};
+
+static void __exit vidtv_bridge_exit(void)
+{
+ platform_driver_unregister(&vidtv_bridge_driver);
+ platform_device_unregister(&vidtv_bridge_dev);
+}
+
+static int __init vidtv_bridge_init(void)
+{
+ int ret;
+
+ ret = platform_device_register(&vidtv_bridge_dev);
+ if (ret)
+ return ret;
+
+ ret = platform_driver_register(&vidtv_bridge_driver);
+ if (ret)
+ platform_device_unregister(&vidtv_bridge_dev);
+
+ return ret;
+}
+
+module_init(vidtv_bridge_init);
+module_exit(vidtv_bridge_exit);
+
+MODULE_DESCRIPTION("Virtual Digital TV Test Driver");
+MODULE_AUTHOR("Daniel W. S. Almeida");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("vidtv");
+MODULE_ALIAS("dvb_vidtv");
diff --git a/drivers/media/test-drivers/vidtv/vidtv_bridge.h b/drivers/media/test-drivers/vidtv/vidtv_bridge.h
new file mode 100644
index 0000000..2528ada
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_bridge.h
@@ -0,0 +1,65 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * The Virtual DTV test driver 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.
+ *
+ * When this module is loaded, it will attempt to modprobe 'dvb_vidtv_tuner' and 'dvb_vidtv_demod'.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#ifndef VIDTV_BRIDGE_H
+#define VIDTV_BRIDGE_H
+
+/*
+ * For now, only one frontend is supported. See vidtv_start_streaming()
+ */
+#define NUM_FE 1
+
+#include <linux/i2c.h>
+#include <linux/platform_device.h>
+#include <linux/types.h>
+
+#include <media/dmxdev.h>
+#include <media/dvb_demux.h>
+#include <media/dvb_frontend.h>
+
+#include "vidtv_mux.h"
+
+/**
+ * struct vidtv_dvb - Vidtv bridge state
+ * @pdev: The platform device. Obtained when the bridge is probed.
+ * @fe: The frontends. Obtained when probing the demodulator modules.
+ * @adapter: Represents a DTV adapter. See 'dvb_register_adapter'.
+ * @demux: The demux used by the dvb_dmx_swfilter_packets() call.
+ * @dmx_dev: Represents a demux device.
+ * @dmx_fe: The frontends associated with the demux.
+ * @i2c_adapter: The i2c_adapter associated with the bridge driver.
+ * @i2c_client_demod: The i2c_clients associated with the demodulator modules.
+ * @i2c_client_tuner: The i2c_clients associated with the tuner modules.
+ * @nfeeds: The number of feeds active.
+ * @feed_lock: Protects access to the start/stop stream logic/data.
+ * @streaming: Whether we are streaming now.
+ * @mux: The abstraction responsible for delivering MPEG TS packets to the bridge.
+ */
+struct vidtv_dvb {
+ struct platform_device *pdev;
+ struct dvb_frontend *fe[NUM_FE];
+ struct dvb_adapter adapter;
+ struct dvb_demux demux;
+ struct dmxdev dmx_dev;
+ struct dmx_frontend dmx_fe[NUM_FE];
+ struct i2c_adapter i2c_adapter;
+ struct i2c_client *i2c_client_demod[NUM_FE];
+ struct i2c_client *i2c_client_tuner[NUM_FE];
+
+ u32 nfeeds;
+ struct mutex feed_lock; /* Protects access to the start/stop stream logic/data. */
+
+ bool streaming;
+
+ struct vidtv_mux *mux;
+};
+
+#endif // VIDTV_BRIDG_H
diff --git a/drivers/media/test-drivers/vidtv/vidtv_channel.c b/drivers/media/test-drivers/vidtv/vidtv_channel.c
new file mode 100644
index 0000000..7838e62
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_channel.c
@@ -0,0 +1,546 @@
+// 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 code for a 'channel' abstraction.
+ *
+ * When vidtv boots, it will create some hardcoded channels.
+ * Their services will be concatenated to populate the SDT.
+ * Their programs will be concatenated to populate the PAT
+ * Their events will be concatenated to populate the EIT
+ * For each program in the PAT, a PMT section will be created
+ * The PMT section for a channel will be assigned its streams.
+ * Every stream will have its corresponding encoder polled to produce TS packets
+ * These packets may be interleaved by the mux and then delivered to the bridge
+ *
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#include <linux/dev_printk.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include "vidtv_channel.h"
+#include "vidtv_common.h"
+#include "vidtv_encoder.h"
+#include "vidtv_mux.h"
+#include "vidtv_psi.h"
+#include "vidtv_s302m.h"
+
+static void vidtv_channel_encoder_destroy(struct vidtv_encoder *e)
+{
+ struct vidtv_encoder *tmp = NULL;
+ struct vidtv_encoder *curr = e;
+
+ while (curr) {
+ /* forward the call to the derived type */
+ tmp = curr;
+ curr = curr->next;
+ tmp->destroy(tmp);
+ }
+}
+
+#define ENCODING_ISO8859_15 "\x0b"
+#define TS_NIT_PID 0x10
+
+/*
+ * init an audio only channel with a s302m encoder
+ */
+struct vidtv_channel
+*vidtv_channel_s302m_init(struct vidtv_channel *head, u16 transport_stream_id)
+{
+ const __be32 s302m_fid = cpu_to_be32(VIDTV_S302M_FORMAT_IDENTIFIER);
+ char *event_text = ENCODING_ISO8859_15 "Bagatelle No. 25 in A minor for solo piano, also known as F\xfcr Elise, composed by Ludwig van Beethoven";
+ char *event_name = ENCODING_ISO8859_15 "Ludwig van Beethoven: F\xfcr Elise";
+ struct vidtv_s302m_encoder_init_args encoder_args = {};
+ char *iso_language_code = ENCODING_ISO8859_15 "eng";
+ char *provider = ENCODING_ISO8859_15 "LinuxTV.org";
+ char *name = ENCODING_ISO8859_15 "Beethoven";
+ const u16 s302m_es_pid = 0x111; /* packet id for the ES */
+ const u16 s302m_program_pid = 0x101; /* packet id for PMT*/
+ const u16 s302m_service_id = 0x880;
+ const u16 s302m_program_num = 0x880;
+ const u16 s302m_beethoven_event_id = 1;
+ struct vidtv_channel *s302m;
+
+ s302m = kzalloc(sizeof(*s302m), GFP_KERNEL);
+ if (!s302m)
+ return NULL;
+
+ s302m->name = kstrdup(name, GFP_KERNEL);
+ if (!s302m->name)
+ goto free_s302m;
+
+ s302m->service = vidtv_psi_sdt_service_init(NULL, s302m_service_id, false, true);
+ if (!s302m->service)
+ goto free_name;
+
+ s302m->service->descriptor = (struct vidtv_psi_desc *)
+ vidtv_psi_service_desc_init(NULL,
+ DIGITAL_RADIO_SOUND_SERVICE,
+ name,
+ provider);
+ if (!s302m->service->descriptor)
+ goto free_service;
+
+ s302m->transport_stream_id = transport_stream_id;
+
+ s302m->program = vidtv_psi_pat_program_init(NULL,
+ s302m_service_id,
+ s302m_program_pid);
+ if (!s302m->program)
+ goto free_service;
+
+ s302m->program_num = s302m_program_num;
+
+ s302m->streams = vidtv_psi_pmt_stream_init(NULL,
+ STREAM_PRIVATE_DATA,
+ s302m_es_pid);
+ if (!s302m->streams)
+ goto free_program;
+
+ s302m->streams->descriptor = (struct vidtv_psi_desc *)
+ vidtv_psi_registration_desc_init(NULL,
+ s302m_fid,
+ NULL,
+ 0);
+ if (!s302m->streams->descriptor)
+ goto free_streams;
+
+ encoder_args.es_pid = s302m_es_pid;
+
+ s302m->encoders = vidtv_s302m_encoder_init(encoder_args);
+ if (!s302m->encoders)
+ goto free_streams;
+
+ s302m->events = vidtv_psi_eit_event_init(NULL, s302m_beethoven_event_id);
+ if (!s302m->events)
+ goto free_encoders;
+ s302m->events->descriptor = (struct vidtv_psi_desc *)
+ vidtv_psi_short_event_desc_init(NULL,
+ iso_language_code,
+ event_name,
+ event_text);
+ if (!s302m->events->descriptor)
+ goto free_events;
+
+ if (head) {
+ while (head->next)
+ head = head->next;
+
+ head->next = s302m;
+ }
+
+ return s302m;
+
+free_events:
+ vidtv_psi_eit_event_destroy(s302m->events);
+free_encoders:
+ vidtv_s302m_encoder_destroy(s302m->encoders);
+free_streams:
+ vidtv_psi_pmt_stream_destroy(s302m->streams);
+free_program:
+ vidtv_psi_pat_program_destroy(s302m->program);
+free_service:
+ vidtv_psi_sdt_service_destroy(s302m->service);
+free_name:
+ kfree(s302m->name);
+free_s302m:
+ kfree(s302m);
+
+ return NULL;
+}
+
+static struct vidtv_psi_table_eit_event
+*vidtv_channel_eit_event_cat_into_new(struct vidtv_mux *m)
+{
+ /* Concatenate the events */
+ const struct vidtv_channel *cur_chnl = m->channels;
+ struct vidtv_psi_table_eit_event *curr = NULL;
+ struct vidtv_psi_table_eit_event *head = NULL;
+ struct vidtv_psi_table_eit_event *tail = NULL;
+ struct vidtv_psi_desc *desc = NULL;
+ u16 event_id;
+
+ if (!cur_chnl)
+ return NULL;
+
+ while (cur_chnl) {
+ curr = cur_chnl->events;
+
+ if (!curr)
+ dev_warn_ratelimited(m->dev,
+ "No events found for channel %s\n",
+ cur_chnl->name);
+
+ while (curr) {
+ event_id = be16_to_cpu(curr->event_id);
+ tail = vidtv_psi_eit_event_init(tail, event_id);
+ if (!tail) {
+ vidtv_psi_eit_event_destroy(head);
+ return NULL;
+ }
+
+ desc = vidtv_psi_desc_clone(curr->descriptor);
+ vidtv_psi_desc_assign(&tail->descriptor, desc);
+
+ if (!head)
+ head = tail;
+
+ curr = curr->next;
+ }
+
+ cur_chnl = cur_chnl->next;
+ }
+
+ return head;
+}
+
+static struct vidtv_psi_table_sdt_service
+*vidtv_channel_sdt_serv_cat_into_new(struct vidtv_mux *m)
+{
+ /* Concatenate the services */
+ const struct vidtv_channel *cur_chnl = m->channels;
+
+ struct vidtv_psi_table_sdt_service *curr = NULL;
+ struct vidtv_psi_table_sdt_service *head = NULL;
+ struct vidtv_psi_table_sdt_service *tail = NULL;
+
+ struct vidtv_psi_desc *desc = NULL;
+ u16 service_id;
+
+ if (!cur_chnl)
+ return NULL;
+
+ while (cur_chnl) {
+ curr = cur_chnl->service;
+
+ if (!curr)
+ dev_warn_ratelimited(m->dev,
+ "No services found for channel %s\n",
+ cur_chnl->name);
+
+ while (curr) {
+ service_id = be16_to_cpu(curr->service_id);
+ tail = vidtv_psi_sdt_service_init(tail,
+ service_id,
+ curr->EIT_schedule,
+ curr->EIT_present_following);
+ if (!tail)
+ goto free;
+
+ desc = vidtv_psi_desc_clone(curr->descriptor);
+ if (!desc)
+ goto free_tail;
+ vidtv_psi_desc_assign(&tail->descriptor, desc);
+
+ if (!head)
+ head = tail;
+
+ curr = curr->next;
+ }
+
+ cur_chnl = cur_chnl->next;
+ }
+
+ return head;
+
+free_tail:
+ vidtv_psi_sdt_service_destroy(tail);
+free:
+ vidtv_psi_sdt_service_destroy(head);
+ return NULL;
+}
+
+static struct vidtv_psi_table_pat_program*
+vidtv_channel_pat_prog_cat_into_new(struct vidtv_mux *m)
+{
+ /* Concatenate the programs */
+ const struct vidtv_channel *cur_chnl = m->channels;
+ struct vidtv_psi_table_pat_program *curr = NULL;
+ struct vidtv_psi_table_pat_program *head = NULL;
+ struct vidtv_psi_table_pat_program *tail = NULL;
+ u16 serv_id;
+ u16 pid;
+
+ if (!cur_chnl)
+ return NULL;
+
+ while (cur_chnl) {
+ curr = cur_chnl->program;
+
+ if (!curr)
+ dev_warn_ratelimited(m->dev,
+ "No programs found for channel %s\n",
+ cur_chnl->name);
+
+ while (curr) {
+ serv_id = be16_to_cpu(curr->service_id);
+ pid = vidtv_psi_get_pat_program_pid(curr);
+ tail = vidtv_psi_pat_program_init(tail,
+ serv_id,
+ pid);
+ if (!tail) {
+ vidtv_psi_pat_program_destroy(head);
+ return NULL;
+ }
+
+ if (!head)
+ head = tail;
+
+ curr = curr->next;
+ }
+
+ cur_chnl = cur_chnl->next;
+ }
+ /* Add the NIT table */
+ vidtv_psi_pat_program_init(tail, 0, TS_NIT_PID);
+
+ return head;
+}
+
+/*
+ * Match channels to their respective PMT sections, then assign the
+ * streams
+ */
+static void
+vidtv_channel_pmt_match_sections(struct vidtv_channel *channels,
+ struct vidtv_psi_table_pmt **sections,
+ u32 nsections)
+{
+ struct vidtv_psi_table_pmt *curr_section = NULL;
+ struct vidtv_psi_table_pmt_stream *head = NULL;
+ struct vidtv_psi_table_pmt_stream *tail = NULL;
+ struct vidtv_psi_table_pmt_stream *s = NULL;
+ struct vidtv_channel *cur_chnl = channels;
+ struct vidtv_psi_desc *desc = NULL;
+ u16 e_pid; /* elementary stream pid */
+ u16 curr_id;
+ u32 j;
+
+ while (cur_chnl) {
+ for (j = 0; j < nsections; ++j) {
+ curr_section = sections[j];
+
+ if (!curr_section)
+ continue;
+
+ curr_id = be16_to_cpu(curr_section->header.id);
+
+ /* we got a match */
+ if (curr_id == cur_chnl->program_num) {
+ s = cur_chnl->streams;
+
+ /* clone the streams for the PMT */
+ while (s) {
+ e_pid = vidtv_psi_pmt_stream_get_elem_pid(s);
+ tail = vidtv_psi_pmt_stream_init(tail,
+ s->type,
+ e_pid);
+
+ if (!head)
+ head = tail;
+
+ desc = vidtv_psi_desc_clone(s->descriptor);
+ vidtv_psi_desc_assign(&tail->descriptor,
+ desc);
+
+ s = s->next;
+ }
+
+ vidtv_psi_pmt_stream_assign(curr_section, head);
+ break;
+ }
+ }
+
+ cur_chnl = cur_chnl->next;
+ }
+}
+
+static void
+vidtv_channel_destroy_service_list(struct vidtv_psi_desc_service_list_entry *e)
+{
+ struct vidtv_psi_desc_service_list_entry *tmp;
+
+ while (e) {
+ tmp = e;
+ e = e->next;
+ kfree(tmp);
+ }
+}
+
+static struct vidtv_psi_desc_service_list_entry
+*vidtv_channel_build_service_list(struct vidtv_psi_table_sdt_service *s)
+{
+ struct vidtv_psi_desc_service_list_entry *curr_e = NULL;
+ struct vidtv_psi_desc_service_list_entry *head_e = NULL;
+ struct vidtv_psi_desc_service_list_entry *prev_e = NULL;
+ struct vidtv_psi_desc *desc = s->descriptor;
+ struct vidtv_psi_desc_service *s_desc;
+
+ while (s) {
+ while (desc) {
+ if (s->descriptor->type != SERVICE_DESCRIPTOR)
+ goto next_desc;
+
+ s_desc = (struct vidtv_psi_desc_service *)desc;
+
+ curr_e = kzalloc(sizeof(*curr_e), GFP_KERNEL);
+ if (!curr_e) {
+ vidtv_channel_destroy_service_list(head_e);
+ return NULL;
+ }
+
+ curr_e->service_id = s->service_id;
+ curr_e->service_type = s_desc->service_type;
+
+ if (!head_e)
+ head_e = curr_e;
+ if (prev_e)
+ prev_e->next = curr_e;
+
+ prev_e = curr_e;
+
+next_desc:
+ desc = desc->next;
+ }
+ s = s->next;
+ }
+ return head_e;
+}
+
+int vidtv_channel_si_init(struct vidtv_mux *m)
+{
+ struct vidtv_psi_desc_service_list_entry *service_list = NULL;
+ struct vidtv_psi_table_pat_program *programs = NULL;
+ struct vidtv_psi_table_sdt_service *services = NULL;
+ struct vidtv_psi_table_eit_event *events = NULL;
+
+ m->si.pat = vidtv_psi_pat_table_init(m->transport_stream_id);
+ if (!m->si.pat)
+ return -ENOMEM;
+
+ m->si.sdt = vidtv_psi_sdt_table_init(m->network_id,
+ m->transport_stream_id);
+ if (!m->si.sdt)
+ goto free_pat;
+
+ programs = vidtv_channel_pat_prog_cat_into_new(m);
+ if (!programs)
+ goto free_sdt;
+ services = vidtv_channel_sdt_serv_cat_into_new(m);
+ if (!services)
+ goto free_programs;
+
+ events = vidtv_channel_eit_event_cat_into_new(m);
+ if (!events)
+ goto free_services;
+
+ /* look for a service descriptor for every service */
+ service_list = vidtv_channel_build_service_list(services);
+ if (!service_list)
+ goto free_events;
+
+ /* use these descriptors to build the NIT */
+ m->si.nit = vidtv_psi_nit_table_init(m->network_id,
+ m->transport_stream_id,
+ m->network_name,
+ service_list);
+ if (!m->si.nit)
+ goto free_service_list;
+
+ m->si.eit = vidtv_psi_eit_table_init(m->network_id,
+ m->transport_stream_id,
+ programs->service_id);
+ if (!m->si.eit)
+ goto free_nit;
+
+ /* assemble all programs and assign to PAT */
+ vidtv_psi_pat_program_assign(m->si.pat, programs);
+
+ /* assemble all services and assign to SDT */
+ vidtv_psi_sdt_service_assign(m->si.sdt, services);
+
+ /* assemble all events and assign to EIT */
+ vidtv_psi_eit_event_assign(m->si.eit, events);
+
+ m->si.pmt_secs = vidtv_psi_pmt_create_sec_for_each_pat_entry(m->si.pat,
+ m->pcr_pid);
+ if (!m->si.pmt_secs)
+ goto free_eit;
+
+ vidtv_channel_pmt_match_sections(m->channels,
+ m->si.pmt_secs,
+ m->si.pat->num_pmt);
+
+ vidtv_channel_destroy_service_list(service_list);
+
+ return 0;
+
+free_eit:
+ vidtv_psi_eit_table_destroy(m->si.eit);
+free_nit:
+ vidtv_psi_nit_table_destroy(m->si.nit);
+free_service_list:
+ vidtv_channel_destroy_service_list(service_list);
+free_events:
+ vidtv_psi_eit_event_destroy(events);
+free_services:
+ vidtv_psi_sdt_service_destroy(services);
+free_programs:
+ vidtv_psi_pat_program_destroy(programs);
+free_sdt:
+ vidtv_psi_sdt_table_destroy(m->si.sdt);
+free_pat:
+ vidtv_psi_pat_table_destroy(m->si.pat);
+ return 0;
+}
+
+void vidtv_channel_si_destroy(struct vidtv_mux *m)
+{
+ u32 i;
+
+ for (i = 0; i < m->si.pat->num_pmt; ++i)
+ vidtv_psi_pmt_table_destroy(m->si.pmt_secs[i]);
+
+ vidtv_psi_pat_table_destroy(m->si.pat);
+
+ kfree(m->si.pmt_secs);
+ vidtv_psi_sdt_table_destroy(m->si.sdt);
+ vidtv_psi_nit_table_destroy(m->si.nit);
+ vidtv_psi_eit_table_destroy(m->si.eit);
+}
+
+int vidtv_channels_init(struct vidtv_mux *m)
+{
+ /* this is the place to add new 'channels' for vidtv */
+ m->channels = vidtv_channel_s302m_init(NULL, m->transport_stream_id);
+
+ if (!m->channels)
+ return -ENOMEM;
+
+ return 0;
+}
+
+void vidtv_channels_destroy(struct vidtv_mux *m)
+{
+ struct vidtv_channel *curr = m->channels;
+ struct vidtv_channel *tmp = NULL;
+
+ while (curr) {
+ kfree(curr->name);
+ vidtv_psi_sdt_service_destroy(curr->service);
+ vidtv_psi_pat_program_destroy(curr->program);
+ vidtv_psi_pmt_stream_destroy(curr->streams);
+ vidtv_channel_encoder_destroy(curr->encoders);
+ vidtv_psi_eit_event_destroy(curr->events);
+
+ tmp = curr;
+ curr = curr->next;
+ kfree(tmp);
+ }
+}
diff --git a/drivers/media/test-drivers/vidtv/vidtv_channel.h b/drivers/media/test-drivers/vidtv/vidtv_channel.h
new file mode 100644
index 0000000..fff2e50
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_channel.h
@@ -0,0 +1,81 @@
+/* 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 code for a 'channel' abstraction.
+ *
+ * When vidtv boots, it will create some hardcoded channels.
+ * Their services will be concatenated to populate the SDT.
+ * Their programs will be concatenated to populate the PAT
+ * Their events will be concatenated to populate the EIT
+ * For each program in the PAT, a PMT section will be created
+ * The PMT section for a channel will be assigned its streams.
+ * Every stream will have its corresponding encoder polled to produce TS packets
+ * These packets may be interleaved by the mux and then delivered to the bridge
+ *
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#ifndef VIDTV_CHANNEL_H
+#define VIDTV_CHANNEL_H
+
+#include <linux/types.h>
+
+#include "vidtv_encoder.h"
+#include "vidtv_mux.h"
+#include "vidtv_psi.h"
+
+/**
+ * struct vidtv_channel - A 'channel' abstraction
+ *
+ * When vidtv boots, it will create some hardcoded channels.
+ * Their services will be concatenated to populate the SDT.
+ * Their programs will be concatenated to populate the PAT
+ * For each program in the PAT, a PMT section will be created
+ * The PMT section for a channel will be assigned its streams.
+ * Every stream will have its corresponding encoder polled to produce TS packets
+ * These packets may be interleaved by the mux and then delivered to the bridge
+ *
+ * @name: name of the channel
+ * @transport_stream_id: a number to identify the TS, chosen at will.
+ * @service: A _single_ service. Will be concatenated into the SDT.
+ * @program_num: The link between PAT, PMT and SDT.
+ * @program: A _single_ program with one or more streams associated with it.
+ * Will be concatenated into the PAT.
+ * @streams: A stream loop used to populate the PMT section for 'program'
+ * @encoders: A encoder loop. There must be one encoder for each stream.
+ * @events: Optional event information. This will feed into the EIT.
+ * @next: Optionally chain this channel.
+ */
+struct vidtv_channel {
+ char *name;
+ u16 transport_stream_id;
+ struct vidtv_psi_table_sdt_service *service;
+ u16 program_num;
+ struct vidtv_psi_table_pat_program *program;
+ struct vidtv_psi_table_pmt_stream *streams;
+ struct vidtv_encoder *encoders;
+ struct vidtv_psi_table_eit_event *events;
+ struct vidtv_channel *next;
+};
+
+/**
+ * vidtv_channel_si_init - Init the PSI tables from the channels in the mux
+ * @m: The mux containing the channels.
+ */
+int vidtv_channel_si_init(struct vidtv_mux *m);
+void vidtv_channel_si_destroy(struct vidtv_mux *m);
+
+/**
+ * vidtv_channels_init - Init hardcoded, fake 'channels'.
+ * @m: The mux to store the channels into.
+ */
+int vidtv_channels_init(struct vidtv_mux *m);
+struct vidtv_channel
+*vidtv_channel_s302m_init(struct vidtv_channel *head, u16 transport_stream_id);
+void vidtv_channels_destroy(struct vidtv_mux *m);
+
+#endif //VIDTV_CHANNEL_H
diff --git a/drivers/media/test-drivers/vidtv/vidtv_common.c b/drivers/media/test-drivers/vidtv/vidtv_common.c
new file mode 100644
index 0000000..63b3055
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_common.c
@@ -0,0 +1,89 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * The Virtual DVB test driver 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.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+#define pr_fmt(fmt) KBUILD_MODNAME ":%s, %d: " fmt, __func__, __LINE__
+
+#include <linux/printk.h>
+#include <linux/ratelimit.h>
+#include <linux/string.h>
+#include <linux/types.h>
+
+#include "vidtv_common.h"
+
+/**
+ * vidtv_memcpy() - wrapper routine to be used by MPEG-TS
+ * generator, in order to avoid going past the
+ * output buffer.
+ * @to: Starting element to where a MPEG-TS packet will
+ * be copied.
+ * @to_offset: Starting position of the @to buffer to be filled.
+ * @to_size: Size of the @to buffer.
+ * @from: Starting element of the buffer to be copied.
+ * @len: Number of elements to be copy from @from buffer
+ * into @to+ @to_offset buffer.
+ *
+ * Note:
+ * Real digital TV demod drivers should not have memcpy
+ * wrappers. We use it here because emulating MPEG-TS
+ * generation at kernelspace requires some extra care.
+ *
+ * Return:
+ * Returns the number of bytes written
+ */
+u32 vidtv_memcpy(void *to,
+ size_t to_offset,
+ size_t to_size,
+ const void *from,
+ size_t len)
+{
+ if (unlikely(to_offset + len > to_size)) {
+ pr_err_ratelimited("overflow detected, skipping. Try increasing the buffer size. Needed %zu, had %zu\n",
+ to_offset + len,
+ to_size);
+ return 0;
+ }
+
+ memcpy(to + to_offset, from, len);
+ return len;
+}
+
+/**
+ * vidtv_memset() - wrapper routine to be used by MPEG-TS
+ * generator, in order to avoid going past the
+ * output buffer.
+ * @to: Starting element to set
+ * @to_offset: Starting position of the @to buffer to be filled.
+ * @to_size: Size of the @to buffer.
+ * @c: The value to set the memory to.
+ * @len: Number of elements to be copy from @from buffer
+ * into @to+ @to_offset buffer.
+ *
+ * Note:
+ * Real digital TV demod drivers should not have memset
+ * wrappers. We use it here because emulating MPEG-TS
+ * generation at kernelspace requires some extra care.
+ *
+ * Return:
+ * Returns the number of bytes written
+ */
+u32 vidtv_memset(void *to,
+ size_t to_offset,
+ size_t to_size,
+ const int c,
+ size_t len)
+{
+ if (unlikely(to_offset + len > to_size)) {
+ pr_err_ratelimited("overflow detected, skipping. Try increasing the buffer size. Needed %zu, had %zu\n",
+ to_offset + len,
+ to_size);
+ return 0;
+ }
+
+ memset(to + to_offset, c, len);
+ return len;
+}
diff --git a/drivers/media/test-drivers/vidtv/vidtv_common.h b/drivers/media/test-drivers/vidtv/vidtv_common.h
new file mode 100644
index 0000000..42f63fd
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_common.h
@@ -0,0 +1,32 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * The Virtual DVB test driver 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.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#ifndef VIDTV_COMMON_H
+#define VIDTV_COMMON_H
+
+#include <linux/types.h>
+
+#define CLOCK_UNIT_90KHZ 90000
+#define CLOCK_UNIT_27MHZ 27000000
+#define VIDTV_SLEEP_USECS 10000
+#define VIDTV_MAX_SLEEP_USECS (2 * VIDTV_SLEEP_USECS)
+
+u32 vidtv_memcpy(void *to,
+ size_t to_offset,
+ size_t to_size,
+ const void *from,
+ size_t len);
+
+u32 vidtv_memset(void *to,
+ size_t to_offset,
+ size_t to_size,
+ int c,
+ size_t len);
+
+#endif // VIDTV_COMMON_H
diff --git a/drivers/media/test-drivers/vidtv/vidtv_demod.c b/drivers/media/test-drivers/vidtv/vidtv_demod.c
new file mode 100644
index 0000000..b7823d9
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_demod.c
@@ -0,0 +1,464 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * The Virtual DVB test driver 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.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ * Based on the example driver written by Emard <emard@softhome.net>
+ */
+
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/printk.h>
+#include <linux/random.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/workqueue.h>
+
+#include <media/dvb_frontend.h>
+
+#include "vidtv_demod.h"
+
+#define POLL_THRD_TIME 2000 /* ms */
+
+static const struct vidtv_demod_cnr_to_qual_s vidtv_demod_c_cnr_2_qual[] = {
+ /* from libdvbv5 source code, in milli db */
+ { QAM_256, FEC_NONE, 34000, 38000},
+ { QAM_64, FEC_NONE, 30000, 34000},
+};
+
+static const struct vidtv_demod_cnr_to_qual_s vidtv_demod_s_cnr_2_qual[] = {
+ /* from libdvbv5 source code, in milli db */
+ { QPSK, FEC_1_2, 7000, 10000},
+ { QPSK, FEC_2_3, 9000, 12000},
+ { QPSK, FEC_3_4, 10000, 13000},
+ { QPSK, FEC_5_6, 11000, 14000},
+ { QPSK, FEC_7_8, 12000, 15000},
+};
+
+static const struct vidtv_demod_cnr_to_qual_s vidtv_demod_s2_cnr_2_qual[] = {
+ /* from libdvbv5 source code, in milli db */
+ { QPSK, FEC_1_2, 9000, 12000},
+ { QPSK, FEC_2_3, 11000, 14000},
+ { QPSK, FEC_3_4, 12000, 15000},
+ { QPSK, FEC_5_6, 12000, 15000},
+ { QPSK, FEC_8_9, 13000, 16000},
+ { QPSK, FEC_9_10, 13500, 16500},
+ { PSK_8, FEC_2_3, 14500, 17500},
+ { PSK_8, FEC_3_4, 16000, 19000},
+ { PSK_8, FEC_5_6, 17500, 20500},
+ { PSK_8, FEC_8_9, 19000, 22000},
+};
+
+static const struct vidtv_demod_cnr_to_qual_s vidtv_demod_t_cnr_2_qual[] = {
+ /* from libdvbv5 source code, in milli db*/
+ { QPSK, FEC_1_2, 4100, 5900},
+ { QPSK, FEC_2_3, 6100, 9600},
+ { QPSK, FEC_3_4, 7200, 12400},
+ { QPSK, FEC_5_6, 8500, 15600},
+ { QPSK, FEC_7_8, 9200, 17500},
+ { QAM_16, FEC_1_2, 9800, 11800},
+ { QAM_16, FEC_2_3, 12100, 15300},
+ { QAM_16, FEC_3_4, 13400, 18100},
+ { QAM_16, FEC_5_6, 14800, 21300},
+ { QAM_16, FEC_7_8, 15700, 23600},
+ { QAM_64, FEC_1_2, 14000, 16000},
+ { QAM_64, FEC_2_3, 19900, 25400},
+ { QAM_64, FEC_3_4, 24900, 27900},
+ { QAM_64, FEC_5_6, 21300, 23300},
+ { QAM_64, FEC_7_8, 22000, 24000},
+};
+
+static const struct vidtv_demod_cnr_to_qual_s *vidtv_match_cnr_s(struct dvb_frontend *fe)
+{
+ const struct vidtv_demod_cnr_to_qual_s *cnr2qual = NULL;
+ struct device *dev = fe->dvb->device;
+ struct dtv_frontend_properties *c;
+ u32 array_size = 0;
+ u32 i;
+
+ c = &fe->dtv_property_cache;
+
+ switch (c->delivery_system) {
+ case SYS_DVBT:
+ case SYS_DVBT2:
+ cnr2qual = vidtv_demod_t_cnr_2_qual;
+ array_size = ARRAY_SIZE(vidtv_demod_t_cnr_2_qual);
+ break;
+
+ case SYS_DVBS:
+ cnr2qual = vidtv_demod_s_cnr_2_qual;
+ array_size = ARRAY_SIZE(vidtv_demod_s_cnr_2_qual);
+ break;
+
+ case SYS_DVBS2:
+ cnr2qual = vidtv_demod_s2_cnr_2_qual;
+ array_size = ARRAY_SIZE(vidtv_demod_s2_cnr_2_qual);
+ break;
+
+ case SYS_DVBC_ANNEX_A:
+ cnr2qual = vidtv_demod_c_cnr_2_qual;
+ array_size = ARRAY_SIZE(vidtv_demod_c_cnr_2_qual);
+ break;
+
+ default:
+ dev_warn_ratelimited(dev,
+ "%s: unsupported delivery system: %u\n",
+ __func__,
+ c->delivery_system);
+ break;
+ }
+
+ for (i = 0; i < array_size; i++)
+ if (cnr2qual[i].modulation == c->modulation &&
+ cnr2qual[i].fec == c->fec_inner)
+ return &cnr2qual[i];
+
+ return NULL; /* not found */
+}
+
+static void vidtv_clean_stats(struct dvb_frontend *fe)
+{
+ struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+ /* Fill the length of each status counter */
+
+ /* Signal is always available */
+ c->strength.len = 1;
+ c->strength.stat[0].scale = FE_SCALE_DECIBEL;
+ c->strength.stat[0].svalue = 0;
+
+ /* Usually available only after Viterbi lock */
+ c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+ c->cnr.stat[0].svalue = 0;
+ c->cnr.len = 1;
+
+ /* Those depends on full lock */
+ c->pre_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+ c->pre_bit_error.stat[0].uvalue = 0;
+ c->pre_bit_error.len = 1;
+ c->pre_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+ c->pre_bit_count.stat[0].uvalue = 0;
+ c->pre_bit_count.len = 1;
+ c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+ c->post_bit_error.stat[0].uvalue = 0;
+ c->post_bit_error.len = 1;
+ c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+ c->post_bit_count.stat[0].uvalue = 0;
+ c->post_bit_count.len = 1;
+ c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+ c->block_error.stat[0].uvalue = 0;
+ c->block_error.len = 1;
+ c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+ c->block_count.stat[0].uvalue = 0;
+ c->block_count.len = 1;
+}
+
+static void vidtv_demod_update_stats(struct dvb_frontend *fe)
+{
+ struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+ struct vidtv_demod_state *state = fe->demodulator_priv;
+ u32 scale;
+
+ if (state->status & FE_HAS_LOCK) {
+ scale = FE_SCALE_COUNTER;
+ c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+ } else {
+ scale = FE_SCALE_NOT_AVAILABLE;
+ c->cnr.stat[0].scale = scale;
+ }
+
+ c->pre_bit_error.stat[0].scale = scale;
+ c->pre_bit_count.stat[0].scale = scale;
+ c->post_bit_error.stat[0].scale = scale;
+ c->post_bit_count.stat[0].scale = scale;
+ c->block_error.stat[0].scale = scale;
+ c->block_count.stat[0].scale = scale;
+
+ /*
+ * Add a 0.5% of randomness at the signal strength and CNR,
+ * and make them different, as we want to have something closer
+ * to a real case scenario.
+ *
+ * Also, usually, signal strength is a negative number in dBm.
+ */
+ c->strength.stat[0].svalue = state->tuner_cnr;
+ c->strength.stat[0].svalue -= prandom_u32_max(state->tuner_cnr / 50);
+ c->strength.stat[0].svalue -= 68000; /* Adjust to a better range */
+
+ c->cnr.stat[0].svalue = state->tuner_cnr;
+ c->cnr.stat[0].svalue -= prandom_u32_max(state->tuner_cnr / 50);
+}
+
+static int vidtv_demod_read_status(struct dvb_frontend *fe,
+ enum fe_status *status)
+{
+ struct vidtv_demod_state *state = fe->demodulator_priv;
+ const struct vidtv_demod_cnr_to_qual_s *cnr2qual = NULL;
+ struct vidtv_demod_config *config = &state->config;
+ u16 snr = 0;
+
+ /* Simulate random lost of signal due to a bad-tuned channel */
+ cnr2qual = vidtv_match_cnr_s(&state->frontend);
+
+ if (cnr2qual && state->tuner_cnr < cnr2qual->cnr_good &&
+ state->frontend.ops.tuner_ops.get_rf_strength) {
+ state->frontend.ops.tuner_ops.get_rf_strength(&state->frontend,
+ &snr);
+
+ if (snr < cnr2qual->cnr_ok) {
+ /* eventually lose the TS lock */
+ if (prandom_u32_max(100) < config->drop_tslock_prob_on_low_snr)
+ state->status = 0;
+ } else {
+ /* recover if the signal improves */
+ if (prandom_u32_max(100) <
+ config->recover_tslock_prob_on_good_snr)
+ state->status = FE_HAS_SIGNAL |
+ FE_HAS_CARRIER |
+ FE_HAS_VITERBI |
+ FE_HAS_SYNC |
+ FE_HAS_LOCK;
+ }
+ }
+
+ vidtv_demod_update_stats(&state->frontend);
+
+ *status = state->status;
+
+ return 0;
+}
+
+static int vidtv_demod_read_signal_strength(struct dvb_frontend *fe,
+ u16 *strength)
+{
+ struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+
+ *strength = c->strength.stat[0].uvalue;
+
+ return 0;
+}
+
+/*
+ * NOTE:
+ * This is implemented here just to be used as an example for real
+ * demod drivers.
+ *
+ * Should only be implemented if it actually reads something from the hardware.
+ * Also, it should check for the locks, in order to avoid report wrong data
+ * to userspace.
+ */
+static int vidtv_demod_get_frontend(struct dvb_frontend *fe,
+ struct dtv_frontend_properties *p)
+{
+ return 0;
+}
+
+static int vidtv_demod_set_frontend(struct dvb_frontend *fe)
+{
+ struct vidtv_demod_state *state = fe->demodulator_priv;
+ u32 tuner_status = 0;
+ int ret;
+
+ if (!fe->ops.tuner_ops.set_params)
+ return 0;
+
+ fe->ops.tuner_ops.set_params(fe);
+
+ /* store the CNR returned by the tuner */
+ ret = fe->ops.tuner_ops.get_rf_strength(fe, &state->tuner_cnr);
+ if (ret < 0)
+ return ret;
+
+ fe->ops.tuner_ops.get_status(fe, &tuner_status);
+ state->status = (state->tuner_cnr > 0) ? FE_HAS_SIGNAL |
+ FE_HAS_CARRIER |
+ FE_HAS_VITERBI |
+ FE_HAS_SYNC |
+ FE_HAS_LOCK :
+ 0;
+
+ vidtv_demod_update_stats(fe);
+
+ if (fe->ops.i2c_gate_ctrl)
+ fe->ops.i2c_gate_ctrl(fe, 0);
+
+ return 0;
+}
+
+/*
+ * NOTE:
+ * This is implemented here just to be used as an example for real
+ * demod drivers.
+ *
+ * Should only be implemented if the demod has support for DVB-S or DVB-S2
+ */
+static int vidtv_demod_set_tone(struct dvb_frontend *fe,
+ enum fe_sec_tone_mode tone)
+{
+ return 0;
+}
+
+/*
+ * NOTE:
+ * This is implemented here just to be used as an example for real
+ * demod drivers.
+ *
+ * Should only be implemented if the demod has support for DVB-S or DVB-S2
+ */
+static int vidtv_demod_set_voltage(struct dvb_frontend *fe,
+ enum fe_sec_voltage voltage)
+{
+ return 0;
+}
+
+/*
+ * NOTE:
+ * This is implemented here just to be used as an example for real
+ * demod drivers.
+ *
+ * Should only be implemented if the demod has support for DVB-S or DVB-S2
+ */
+static int vidtv_send_diseqc_msg(struct dvb_frontend *fe,
+ struct dvb_diseqc_master_cmd *cmd)
+{
+ return 0;
+}
+
+/*
+ * NOTE:
+ * This is implemented here just to be used as an example for real
+ * demod drivers.
+ *
+ * Should only be implemented if the demod has support for DVB-S or DVB-S2
+ */
+static int vidtv_diseqc_send_burst(struct dvb_frontend *fe,
+ enum fe_sec_mini_cmd burst)
+{
+ return 0;
+}
+
+static void vidtv_demod_release(struct dvb_frontend *fe)
+{
+ struct vidtv_demod_state *state = fe->demodulator_priv;
+
+ kfree(state);
+}
+
+static const struct dvb_frontend_ops vidtv_demod_ops = {
+ .delsys = {
+ SYS_DVBT,
+ SYS_DVBT2,
+ SYS_DVBC_ANNEX_A,
+ SYS_DVBS,
+ SYS_DVBS2,
+ },
+
+ .info = {
+ .name = "Dummy demod for DVB-T/T2/C/S/S2",
+ .frequency_min_hz = 51 * MHz,
+ .frequency_max_hz = 2150 * MHz,
+ .frequency_stepsize_hz = 62500,
+ .frequency_tolerance_hz = 29500 * kHz,
+ .symbol_rate_min = 1000000,
+ .symbol_rate_max = 45000000,
+
+ .caps = FE_CAN_FEC_1_2 |
+ FE_CAN_FEC_2_3 |
+ FE_CAN_FEC_3_4 |
+ FE_CAN_FEC_4_5 |
+ FE_CAN_FEC_5_6 |
+ FE_CAN_FEC_6_7 |
+ FE_CAN_FEC_7_8 |
+ FE_CAN_FEC_8_9 |
+ FE_CAN_QAM_16 |
+ FE_CAN_QAM_64 |
+ FE_CAN_QAM_32 |
+ FE_CAN_QAM_128 |
+ FE_CAN_QAM_256 |
+ FE_CAN_QAM_AUTO |
+ FE_CAN_QPSK |
+ FE_CAN_FEC_AUTO |
+ FE_CAN_INVERSION_AUTO |
+ FE_CAN_TRANSMISSION_MODE_AUTO |
+ FE_CAN_GUARD_INTERVAL_AUTO |
+ FE_CAN_HIERARCHY_AUTO,
+ },
+
+ .release = vidtv_demod_release,
+
+ .set_frontend = vidtv_demod_set_frontend,
+ .get_frontend = vidtv_demod_get_frontend,
+
+ .read_status = vidtv_demod_read_status,
+ .read_signal_strength = vidtv_demod_read_signal_strength,
+
+ /* For DVB-S/S2 */
+ .set_voltage = vidtv_demod_set_voltage,
+ .set_tone = vidtv_demod_set_tone,
+ .diseqc_send_master_cmd = vidtv_send_diseqc_msg,
+ .diseqc_send_burst = vidtv_diseqc_send_burst,
+
+};
+
+static const struct i2c_device_id vidtv_demod_i2c_id_table[] = {
+ {"dvb_vidtv_demod", 0},
+ {}
+};
+MODULE_DEVICE_TABLE(i2c, vidtv_demod_i2c_id_table);
+
+static int vidtv_demod_i2c_probe(struct i2c_client *client,
+ const struct i2c_device_id *id)
+{
+ struct vidtv_tuner_config *config = client->dev.platform_data;
+ struct vidtv_demod_state *state;
+
+ /* allocate memory for the internal state */
+ state = kzalloc(sizeof(*state), GFP_KERNEL);
+ if (!state)
+ return -ENOMEM;
+
+ /* create dvb_frontend */
+ memcpy(&state->frontend.ops,
+ &vidtv_demod_ops,
+ sizeof(struct dvb_frontend_ops));
+
+ memcpy(&state->config, config, sizeof(state->config));
+
+ state->frontend.demodulator_priv = state;
+ i2c_set_clientdata(client, state);
+
+ vidtv_clean_stats(&state->frontend);
+
+ return 0;
+}
+
+static int vidtv_demod_i2c_remove(struct i2c_client *client)
+{
+ struct vidtv_demod_state *state = i2c_get_clientdata(client);
+
+ kfree(state);
+
+ return 0;
+}
+
+static struct i2c_driver vidtv_demod_i2c_driver = {
+ .driver = {
+ .name = "dvb_vidtv_demod",
+ .suppress_bind_attrs = true,
+ },
+ .probe = vidtv_demod_i2c_probe,
+ .remove = vidtv_demod_i2c_remove,
+ .id_table = vidtv_demod_i2c_id_table,
+};
+
+module_i2c_driver(vidtv_demod_i2c_driver);
+
+MODULE_DESCRIPTION("Virtual DVB Demodulator Driver");
+MODULE_AUTHOR("Daniel W. S. Almeida");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/test-drivers/vidtv/vidtv_demod.h b/drivers/media/test-drivers/vidtv/vidtv_demod.h
new file mode 100644
index 0000000..2b84046
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_demod.h
@@ -0,0 +1,68 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * The Virtual DTV test driver 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.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ * Based on the example driver written by Emard <emard@softhome.net>
+ */
+
+#ifndef VIDTV_DEMOD_H
+#define VIDTV_DEMOD_H
+
+#include <linux/dvb/frontend.h>
+
+#include <media/dvb_frontend.h>
+
+/**
+ * struct vidtv_demod_cnr_to_qual_s - Map CNR values to a given combination of
+ * modulation and fec_inner
+ * @modulation: see enum fe_modulation
+ * @fec: see enum fe_fec_rate
+ * @cnr_ok: S/N threshold to consider the signal as OK. Below that, there's
+ * a chance of losing sync.
+ * @cnr_good: S/N threshold to consider the signal strong.
+ *
+ * This struct matches values for 'good' and 'ok' CNRs given the combination
+ * of modulation and fec_inner in use. We might simulate some noise if the
+ * signal quality is not too good.
+ *
+ * The values were taken from libdvbv5.
+ */
+struct vidtv_demod_cnr_to_qual_s {
+ u32 modulation;
+ u32 fec;
+ u32 cnr_ok;
+ u32 cnr_good;
+};
+
+/**
+ * struct vidtv_demod_config - Configuration used to init the demod
+ * @drop_tslock_prob_on_low_snr: probability of losing the lock due to low snr
+ * @recover_tslock_prob_on_good_snr: probability of recovering when the signal
+ * improves
+ *
+ * The configuration used to init the demodulator module, usually filled
+ * by a bridge driver. For vidtv, this is filled by vidtv_bridge before the
+ * demodulator module is probed.
+ */
+struct vidtv_demod_config {
+ u8 drop_tslock_prob_on_low_snr;
+ u8 recover_tslock_prob_on_good_snr;
+};
+
+/**
+ * struct vidtv_demod_state - The demodulator state
+ * @frontend: The frontend structure allocated by the demod.
+ * @config: The config used to init the demod.
+ * @status: the demod status.
+ * @tuner_cnr: current S/N ratio for the signal carrier
+ */
+struct vidtv_demod_state {
+ struct dvb_frontend frontend;
+ struct vidtv_demod_config config;
+ enum fe_status status;
+ u16 tuner_cnr;
+};
+#endif // VIDTV_DEMOD_H
diff --git a/drivers/media/test-drivers/vidtv/vidtv_encoder.h b/drivers/media/test-drivers/vidtv/vidtv_encoder.h
new file mode 100644
index 0000000..50e3cf4
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_encoder.h
@@ -0,0 +1,165 @@
+/* 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 a generic encoder type that can provide data for a stream
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#ifndef VIDTV_ENCODER_H
+#define VIDTV_ENCODER_H
+
+#include <linux/types.h>
+
+enum vidtv_encoder_id {
+ /* add IDs here when implementing new encoders */
+ S302M,
+};
+
+struct vidtv_access_unit {
+ u32 num_samples;
+ u64 pts;
+ u64 dts;
+ u32 nbytes;
+ u32 offset;
+ struct vidtv_access_unit *next;
+};
+
+/* Some musical notes, used by a tone generator. Values are in Hz */
+enum musical_notes {
+ NOTE_SILENT = 0,
+
+ NOTE_C_2 = 65,
+ NOTE_CS_2 = 69,
+ NOTE_D_2 = 73,
+ NOTE_DS_2 = 78,
+ NOTE_E_2 = 82,
+ NOTE_F_2 = 87,
+ NOTE_FS_2 = 93,
+ NOTE_G_2 = 98,
+ NOTE_GS_2 = 104,
+ NOTE_A_2 = 110,
+ NOTE_AS_2 = 117,
+ NOTE_B_2 = 123,
+ NOTE_C_3 = 131,
+ NOTE_CS_3 = 139,
+ NOTE_D_3 = 147,
+ NOTE_DS_3 = 156,
+ NOTE_E_3 = 165,
+ NOTE_F_3 = 175,
+ NOTE_FS_3 = 185,
+ NOTE_G_3 = 196,
+ NOTE_GS_3 = 208,
+ NOTE_A_3 = 220,
+ NOTE_AS_3 = 233,
+ NOTE_B_3 = 247,
+ NOTE_C_4 = 262,
+ NOTE_CS_4 = 277,
+ NOTE_D_4 = 294,
+ NOTE_DS_4 = 311,
+ NOTE_E_4 = 330,
+ NOTE_F_4 = 349,
+ NOTE_FS_4 = 370,
+ NOTE_G_4 = 392,
+ NOTE_GS_4 = 415,
+ NOTE_A_4 = 440,
+ NOTE_AS_4 = 466,
+ NOTE_B_4 = 494,
+ NOTE_C_5 = 523,
+ NOTE_CS_5 = 554,
+ NOTE_D_5 = 587,
+ NOTE_DS_5 = 622,
+ NOTE_E_5 = 659,
+ NOTE_F_5 = 698,
+ NOTE_FS_5 = 740,
+ NOTE_G_5 = 784,
+ NOTE_GS_5 = 831,
+ NOTE_A_5 = 880,
+ NOTE_AS_5 = 932,
+ NOTE_B_5 = 988,
+ NOTE_C_6 = 1047,
+ NOTE_CS_6 = 1109,
+ NOTE_D_6 = 1175,
+ NOTE_DS_6 = 1245,
+ NOTE_E_6 = 1319,
+ NOTE_F_6 = 1397,
+ NOTE_FS_6 = 1480,
+ NOTE_G_6 = 1568,
+ NOTE_GS_6 = 1661,
+ NOTE_A_6 = 1760,
+ NOTE_AS_6 = 1865,
+ NOTE_B_6 = 1976,
+ NOTE_C_7 = 2093
+};
+
+/**
+ * struct vidtv_encoder - A generic encoder type.
+ * @id: So we can cast to a concrete implementation when needed.
+ * @name: Usually the same as the stream name.
+ * @encoder_buf: The encoder internal buffer for the access units.
+ * @encoder_buf_sz: The encoder buffer size, in bytes
+ * @encoder_buf_offset: Our byte position in the encoder buffer.
+ * @sample_count: How many samples we have encoded in total.
+ * @access_units: encoder payload units, used for clock references
+ * @src_buf: The source of raw data to be encoded, encoder might set a
+ * default if null.
+ * @src_buf_sz: size of @src_buf.
+ * @src_buf_offset: Our position in the source buffer.
+ * @is_video_encoder: Whether this a video encoder (as opposed to audio)
+ * @ctx: Encoder-specific state.
+ * @stream_id: Examples: Audio streams (0xc0-0xdf), Video streams
+ * (0xe0-0xef).
+ * @es_pid: The TS PID to use for the elementary stream in this encoder.
+ * @encode: Prepare enough AUs for the given amount of time.
+ * @clear: Clear the encoder output.
+ * @sync: Attempt to synchronize with this encoder.
+ * @sampling_rate_hz: The sampling rate (or fps, if video) used.
+ * @last_sample_cb: Called when the encoder runs out of data.This is
+ * so the source can read data in a
+ * piecemeal fashion instead of having to
+ * provide it all at once.
+ * @destroy: Destroy this encoder, freeing allocated resources.
+ * @next: Next in the chain
+ */
+struct vidtv_encoder {
+ enum vidtv_encoder_id id;
+ char *name;
+
+ u8 *encoder_buf;
+ u32 encoder_buf_sz;
+ u32 encoder_buf_offset;
+
+ u64 sample_count;
+
+ struct vidtv_access_unit *access_units;
+
+ void *src_buf;
+ u32 src_buf_sz;
+ u32 src_buf_offset;
+
+ bool is_video_encoder;
+ void *ctx;
+
+ __be16 stream_id;
+
+ __be16 es_pid;
+
+ void *(*encode)(struct vidtv_encoder *e);
+
+ u32 (*clear)(struct vidtv_encoder *e);
+
+ struct vidtv_encoder *sync;
+
+ u32 sampling_rate_hz;
+
+ void (*last_sample_cb)(u32 sample_no);
+
+ void (*destroy)(struct vidtv_encoder *e);
+
+ struct vidtv_encoder *next;
+};
+
+#endif /* VIDTV_ENCODER_H */
diff --git a/drivers/media/test-drivers/vidtv/vidtv_mux.c b/drivers/media/test-drivers/vidtv/vidtv_mux.c
new file mode 100644
index 0000000..b51e6a3
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_mux.c
@@ -0,0 +1,546 @@
+// 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 multiplexer logic for TS packets from different
+ * elementary streams
+ *
+ * Loosely based on libavcodec/mpegtsenc.c
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#include <linux/delay.h>
+#include <linux/dev_printk.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+#include <linux/vmalloc.h>
+
+#include "vidtv_channel.h"
+#include "vidtv_common.h"
+#include "vidtv_encoder.h"
+#include "vidtv_mux.h"
+#include "vidtv_pes.h"
+#include "vidtv_psi.h"
+#include "vidtv_ts.h"
+
+static struct vidtv_mux_pid_ctx
+*vidtv_mux_get_pid_ctx(struct vidtv_mux *m, u16 pid)
+{
+ struct vidtv_mux_pid_ctx *ctx;
+
+ hash_for_each_possible(m->pid_ctx, ctx, h, pid)
+ if (ctx->pid == pid)
+ return ctx;
+ return NULL;
+}
+
+static struct vidtv_mux_pid_ctx
+*vidtv_mux_create_pid_ctx_once(struct vidtv_mux *m, u16 pid)
+{
+ struct vidtv_mux_pid_ctx *ctx;
+
+ ctx = vidtv_mux_get_pid_ctx(m, pid);
+ if (ctx)
+ return ctx;
+
+ ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+ if (!ctx)
+ return NULL;
+
+ ctx->pid = pid;
+ ctx->cc = 0;
+ hash_add(m->pid_ctx, &ctx->h, pid);
+
+ return ctx;
+}
+
+static void vidtv_mux_pid_ctx_destroy(struct vidtv_mux *m)
+{
+ struct vidtv_mux_pid_ctx *ctx;
+ struct hlist_node *tmp;
+ int bkt;
+
+ hash_for_each_safe(m->pid_ctx, bkt, tmp, ctx, h) {
+ hash_del(&ctx->h);
+ kfree(ctx);
+ }
+}
+
+static int vidtv_mux_pid_ctx_init(struct vidtv_mux *m)
+{
+ struct vidtv_psi_table_pat_program *p = m->si.pat->program;
+ u16 pid;
+
+ hash_init(m->pid_ctx);
+ /* push the pcr pid ctx */
+ if (!vidtv_mux_create_pid_ctx_once(m, m->pcr_pid))
+ return -ENOMEM;
+ /* push the NULL packet pid ctx */
+ if (!vidtv_mux_create_pid_ctx_once(m, TS_NULL_PACKET_PID))
+ goto free;
+ /* push the PAT pid ctx */
+ if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_PAT_PID))
+ goto free;
+ /* push the SDT pid ctx */
+ if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_SDT_PID))
+ goto free;
+ /* push the NIT pid ctx */
+ if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_NIT_PID))
+ goto free;
+ /* push the EIT pid ctx */
+ if (!vidtv_mux_create_pid_ctx_once(m, VIDTV_EIT_PID))
+ goto free;
+
+ /* add a ctx for all PMT sections */
+ while (p) {
+ pid = vidtv_psi_get_pat_program_pid(p);
+ vidtv_mux_create_pid_ctx_once(m, pid);
+ p = p->next;
+ }
+
+ return 0;
+
+free:
+ vidtv_mux_pid_ctx_destroy(m);
+ return -ENOMEM;
+}
+
+static void vidtv_mux_update_clk(struct vidtv_mux *m)
+{
+ /* call this at every thread iteration */
+ u64 elapsed_time;
+
+ m->timing.past_jiffies = m->timing.current_jiffies;
+ m->timing.current_jiffies = get_jiffies_64();
+
+ elapsed_time = jiffies_to_usecs(m->timing.current_jiffies -
+ m->timing.past_jiffies);
+
+ /* update the 27Mhz clock proportionally to the elapsed time */
+ m->timing.clk += (CLOCK_UNIT_27MHZ / USEC_PER_SEC) * elapsed_time;
+}
+
+static u32 vidtv_mux_push_si(struct vidtv_mux *m)
+{
+ struct vidtv_psi_pat_write_args pat_args = {
+ .buf = m->mux_buf,
+ .buf_sz = m->mux_buf_sz,
+ .pat = m->si.pat,
+ };
+ struct vidtv_psi_pmt_write_args pmt_args = {
+ .buf = m->mux_buf,
+ .buf_sz = m->mux_buf_sz,
+ .pcr_pid = m->pcr_pid,
+ };
+ struct vidtv_psi_sdt_write_args sdt_args = {
+ .buf = m->mux_buf,
+ .buf_sz = m->mux_buf_sz,
+ .sdt = m->si.sdt,
+ };
+ struct vidtv_psi_nit_write_args nit_args = {
+ .buf = m->mux_buf,
+ .buf_sz = m->mux_buf_sz,
+ .nit = m->si.nit,
+
+ };
+ struct vidtv_psi_eit_write_args eit_args = {
+ .buf = m->mux_buf,
+ .buf_sz = m->mux_buf_sz,
+ .eit = m->si.eit,
+ };
+ u32 initial_offset = m->mux_buf_offset;
+ struct vidtv_mux_pid_ctx *pat_ctx;
+ struct vidtv_mux_pid_ctx *pmt_ctx;
+ struct vidtv_mux_pid_ctx *sdt_ctx;
+ struct vidtv_mux_pid_ctx *nit_ctx;
+ struct vidtv_mux_pid_ctx *eit_ctx;
+ u32 nbytes;
+ u16 pmt_pid;
+ u32 i;
+
+ pat_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_PAT_PID);
+ sdt_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_SDT_PID);
+ nit_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_NIT_PID);
+ eit_ctx = vidtv_mux_get_pid_ctx(m, VIDTV_EIT_PID);
+
+ pat_args.offset = m->mux_buf_offset;
+ pat_args.continuity_counter = &pat_ctx->cc;
+
+ m->mux_buf_offset += vidtv_psi_pat_write_into(&pat_args);
+
+ for (i = 0; i < m->si.pat->num_pmt; ++i) {
+ pmt_pid = vidtv_psi_pmt_get_pid(m->si.pmt_secs[i],
+ m->si.pat);
+
+ if (pmt_pid > TS_LAST_VALID_PID) {
+ dev_warn_ratelimited(m->dev,
+ "PID: %d not found\n", pmt_pid);
+ continue;
+ }
+
+ pmt_ctx = vidtv_mux_get_pid_ctx(m, pmt_pid);
+
+ pmt_args.offset = m->mux_buf_offset;
+ pmt_args.pmt = m->si.pmt_secs[i];
+ pmt_args.pid = pmt_pid;
+ pmt_args.continuity_counter = &pmt_ctx->cc;
+
+ /* write each section into buffer */
+ m->mux_buf_offset += vidtv_psi_pmt_write_into(&pmt_args);
+ }
+
+ sdt_args.offset = m->mux_buf_offset;
+ sdt_args.continuity_counter = &sdt_ctx->cc;
+
+ m->mux_buf_offset += vidtv_psi_sdt_write_into(&sdt_args);
+
+ nit_args.offset = m->mux_buf_offset;
+ nit_args.continuity_counter = &nit_ctx->cc;
+
+ m->mux_buf_offset += vidtv_psi_nit_write_into(&nit_args);
+
+ eit_args.offset = m->mux_buf_offset;
+ eit_args.continuity_counter = &eit_ctx->cc;
+
+ m->mux_buf_offset += vidtv_psi_eit_write_into(&eit_args);
+
+ nbytes = m->mux_buf_offset - initial_offset;
+
+ m->num_streamed_si++;
+
+ return nbytes;
+}
+
+static u32 vidtv_mux_push_pcr(struct vidtv_mux *m)
+{
+ struct pcr_write_args args = {};
+ struct vidtv_mux_pid_ctx *ctx;
+ u32 nbytes = 0;
+
+ ctx = vidtv_mux_get_pid_ctx(m, m->pcr_pid);
+ args.dest_buf = m->mux_buf;
+ args.pid = m->pcr_pid;
+ args.buf_sz = m->mux_buf_sz;
+ args.continuity_counter = &ctx->cc;
+
+ /* the 27Mhz clock will feed both parts of the PCR bitfield */
+ args.pcr = m->timing.clk;
+
+ nbytes += vidtv_ts_pcr_write_into(args);
+ m->mux_buf_offset += nbytes;
+
+ m->num_streamed_pcr++;
+
+ return nbytes;
+}
+
+static bool vidtv_mux_should_push_pcr(struct vidtv_mux *m)
+{
+ u64 next_pcr_at;
+
+ if (m->num_streamed_pcr == 0)
+ return true;
+
+ next_pcr_at = m->timing.start_jiffies +
+ usecs_to_jiffies(m->num_streamed_pcr *
+ m->timing.pcr_period_usecs);
+
+ return time_after64(m->timing.current_jiffies, next_pcr_at);
+}
+
+static bool vidtv_mux_should_push_si(struct vidtv_mux *m)
+{
+ u64 next_si_at;
+
+ if (m->num_streamed_si == 0)
+ return true;
+
+ next_si_at = m->timing.start_jiffies +
+ usecs_to_jiffies(m->num_streamed_si *
+ m->timing.si_period_usecs);
+
+ return time_after64(m->timing.current_jiffies, next_si_at);
+}
+
+static u32 vidtv_mux_packetize_access_units(struct vidtv_mux *m,
+ struct vidtv_encoder *e)
+{
+ struct pes_write_args args = {
+ .dest_buf = m->mux_buf,
+ .dest_buf_sz = m->mux_buf_sz,
+ .pid = be16_to_cpu(e->es_pid),
+ .encoder_id = e->id,
+ .stream_id = be16_to_cpu(e->stream_id),
+ .send_pts = true, /* forbidden value '01'... */
+ .send_dts = false, /* ...for PTS_DTS flags */
+ };
+ struct vidtv_access_unit *au = e->access_units;
+ u32 initial_offset = m->mux_buf_offset;
+ struct vidtv_mux_pid_ctx *pid_ctx;
+ u32 nbytes = 0;
+ u8 *buf = NULL;
+
+ /* see SMPTE 302M clause 6.4 */
+ if (args.encoder_id == S302M) {
+ args.send_dts = false;
+ args.send_pts = true;
+ }
+
+ pid_ctx = vidtv_mux_create_pid_ctx_once(m, be16_to_cpu(e->es_pid));
+ args.continuity_counter = &pid_ctx->cc;
+
+ while (au) {
+ buf = e->encoder_buf + au->offset;
+ args.from = buf;
+ args.access_unit_len = au->nbytes;
+ args.dest_offset = m->mux_buf_offset;
+ args.pts = au->pts;
+ args.pcr = m->timing.clk;
+
+ m->mux_buf_offset += vidtv_pes_write_into(&args);
+
+ au = au->next;
+ }
+
+ /*
+ * clear the encoder state once the ES data has been written to the mux
+ * buffer
+ */
+ e->clear(e);
+
+ nbytes = m->mux_buf_offset - initial_offset;
+ return nbytes;
+}
+
+static u32 vidtv_mux_poll_encoders(struct vidtv_mux *m)
+{
+ struct vidtv_channel *cur_chnl = m->channels;
+ struct vidtv_encoder *e = NULL;
+ u32 nbytes = 0;
+ u32 au_nbytes;
+
+ while (cur_chnl) {
+ e = cur_chnl->encoders;
+
+ while (e) {
+ e->encode(e);
+ /* get the TS packets into the mux buffer */
+ au_nbytes = vidtv_mux_packetize_access_units(m, e);
+ nbytes += au_nbytes;
+ m->mux_buf_offset += au_nbytes;
+ /* grab next encoder */
+ e = e->next;
+ }
+
+ /* grab the next channel */
+ cur_chnl = cur_chnl->next;
+ }
+
+ return nbytes;
+}
+
+static u32 vidtv_mux_pad_with_nulls(struct vidtv_mux *m, u32 npkts)
+{
+ struct null_packet_write_args args = {
+ .dest_buf = m->mux_buf,
+ .buf_sz = m->mux_buf_sz,
+ .dest_offset = m->mux_buf_offset,
+ };
+ u32 initial_offset = m->mux_buf_offset;
+ struct vidtv_mux_pid_ctx *ctx;
+ u32 nbytes;
+ u32 i;
+
+ ctx = vidtv_mux_get_pid_ctx(m, TS_NULL_PACKET_PID);
+
+ args.continuity_counter = &ctx->cc;
+
+ for (i = 0; i < npkts; ++i) {
+ m->mux_buf_offset += vidtv_ts_null_write_into(args);
+ args.dest_offset = m->mux_buf_offset;
+ }
+
+ nbytes = m->mux_buf_offset - initial_offset;
+
+ /* sanity check */
+ if (nbytes != npkts * TS_PACKET_LEN)
+ dev_err_ratelimited(m->dev, "%d != %d\n",
+ nbytes, npkts * TS_PACKET_LEN);
+
+ return nbytes;
+}
+
+static void vidtv_mux_clear(struct vidtv_mux *m)
+{
+ /* clear the packets currently in the mux */
+ memset(m->mux_buf, 0, m->mux_buf_sz * sizeof(*m->mux_buf));
+ /* point to the beginning of the buffer again */
+ m->mux_buf_offset = 0;
+}
+
+#define ERR_RATE 10000000
+static void vidtv_mux_tick(struct work_struct *work)
+{
+ struct vidtv_mux *m = container_of(work,
+ struct vidtv_mux,
+ mpeg_thread);
+ struct dtv_frontend_properties *c = &m->fe->dtv_property_cache;
+ u32 tot_bits = 0;
+ u32 nbytes;
+ u32 npkts;
+
+ while (m->streaming) {
+ nbytes = 0;
+
+ vidtv_mux_update_clk(m);
+
+ if (vidtv_mux_should_push_pcr(m))
+ nbytes += vidtv_mux_push_pcr(m);
+
+ if (vidtv_mux_should_push_si(m))
+ nbytes += vidtv_mux_push_si(m);
+
+ nbytes += vidtv_mux_poll_encoders(m);
+ nbytes += vidtv_mux_pad_with_nulls(m, 256);
+
+ npkts = nbytes / TS_PACKET_LEN;
+
+ /* if the buffer is not aligned there is a bug somewhere */
+ if (nbytes % TS_PACKET_LEN)
+ dev_err_ratelimited(m->dev, "Misaligned buffer\n");
+
+ if (m->on_new_packets_available_cb)
+ m->on_new_packets_available_cb(m->priv,
+ m->mux_buf,
+ npkts);
+
+ vidtv_mux_clear(m);
+
+ /*
+ * Update bytes and packet counts at DVBv5 stats
+ *
+ * For now, both pre and post bit counts are identical,
+ * but post BER count can be lower than pre BER, if the error
+ * correction logic discards packages.
+ */
+ c->pre_bit_count.stat[0].uvalue = nbytes * 8;
+ c->post_bit_count.stat[0].uvalue = nbytes * 8;
+ c->block_count.stat[0].uvalue += npkts;
+
+ /*
+ * Even without any visible errors for the user, the pre-BER
+ * stats usually have an error range up to 1E-6. So,
+ * add some random error increment count to it.
+ *
+ * Please notice that this is a poor guy's implementation,
+ * as it will produce one corrected bit error every time
+ * ceil(total bytes / ERR_RATE) is incremented, without
+ * any sort of (pseudo-)randomness.
+ */
+ tot_bits += nbytes * 8;
+ if (tot_bits > ERR_RATE) {
+ c->pre_bit_error.stat[0].uvalue++;
+ tot_bits -= ERR_RATE;
+ }
+
+ usleep_range(VIDTV_SLEEP_USECS, VIDTV_MAX_SLEEP_USECS);
+ }
+}
+
+void vidtv_mux_start_thread(struct vidtv_mux *m)
+{
+ if (m->streaming) {
+ dev_warn_ratelimited(m->dev, "Already streaming. Skipping.\n");
+ return;
+ }
+
+ m->streaming = true;
+ m->timing.start_jiffies = get_jiffies_64();
+ schedule_work(&m->mpeg_thread);
+}
+
+void vidtv_mux_stop_thread(struct vidtv_mux *m)
+{
+ if (m->streaming) {
+ m->streaming = false; /* thread will quit */
+ cancel_work_sync(&m->mpeg_thread);
+ }
+}
+
+struct vidtv_mux *vidtv_mux_init(struct dvb_frontend *fe,
+ struct device *dev,
+ struct vidtv_mux_init_args *args)
+{
+ struct vidtv_mux *m;
+
+ m = kzalloc(sizeof(*m), GFP_KERNEL);
+ if (!m)
+ return NULL;
+
+ m->dev = dev;
+ m->fe = fe;
+ m->timing.pcr_period_usecs = args->pcr_period_usecs;
+ m->timing.si_period_usecs = args->si_period_usecs;
+
+ m->mux_rate_kbytes_sec = args->mux_rate_kbytes_sec;
+
+ m->on_new_packets_available_cb = args->on_new_packets_available_cb;
+
+ m->mux_buf = vzalloc(args->mux_buf_sz);
+ if (!m->mux_buf)
+ goto free_mux;
+
+ m->mux_buf_sz = args->mux_buf_sz;
+
+ m->pcr_pid = args->pcr_pid;
+ m->transport_stream_id = args->transport_stream_id;
+ m->priv = args->priv;
+ m->network_id = args->network_id;
+ m->network_name = kstrdup(args->network_name, GFP_KERNEL);
+ m->timing.current_jiffies = get_jiffies_64();
+
+ if (args->channels)
+ m->channels = args->channels;
+ else
+ if (vidtv_channels_init(m) < 0)
+ goto free_mux_buf;
+
+ /* will alloc data for pmt_sections after initializing pat */
+ if (vidtv_channel_si_init(m) < 0)
+ goto free_channels;
+
+ INIT_WORK(&m->mpeg_thread, vidtv_mux_tick);
+
+ if (vidtv_mux_pid_ctx_init(m) < 0)
+ goto free_channel_si;
+
+ return m;
+
+free_channel_si:
+ vidtv_channel_si_destroy(m);
+free_channels:
+ vidtv_channels_destroy(m);
+free_mux_buf:
+ vfree(m->mux_buf);
+free_mux:
+ kfree(m);
+ return NULL;
+}
+
+void vidtv_mux_destroy(struct vidtv_mux *m)
+{
+ vidtv_mux_stop_thread(m);
+ vidtv_mux_pid_ctx_destroy(m);
+ vidtv_channel_si_destroy(m);
+ vidtv_channels_destroy(m);
+ kfree(m->network_name);
+ vfree(m->mux_buf);
+ kfree(m);
+}
diff --git a/drivers/media/test-drivers/vidtv/vidtv_mux.h b/drivers/media/test-drivers/vidtv/vidtv_mux.h
new file mode 100644
index 0000000..ad82eb7
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_mux.h
@@ -0,0 +1,182 @@
+/* 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 muxer logic for TS packets from different
+ * elementary streams.
+ *
+ * Loosely based on libavcodec/mpegtsenc.c
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#ifndef VIDTV_MUX_H
+#define VIDTV_MUX_H
+
+#include <linux/hashtable.h>
+#include <linux/types.h>
+#include <linux/workqueue.h>
+
+#include <media/dvb_frontend.h>
+
+#include "vidtv_psi.h"
+
+/**
+ * struct vidtv_mux_timing - Timing related information
+ *
+ * This is used to decide when PCR or PSI packets should be sent. This will also
+ * provide storage for the clock, which is used to compute the value for the PCR.
+ *
+ * @start_jiffies: The value of 'jiffies' when we started the mux thread.
+ * @current_jiffies: The value of 'jiffies' for the current iteration.
+ * @past_jiffies: The value of 'jiffies' for the past iteration.
+ * @clk: A 27Mhz clock from which we will drive the PCR. Updated proportionally
+ * on every iteration.
+ * @pcr_period_usecs: How often we should send PCR packets.
+ * @si_period_usecs: How often we should send PSI packets.
+ */
+struct vidtv_mux_timing {
+ u64 start_jiffies;
+ u64 current_jiffies;
+ u64 past_jiffies;
+
+ u64 clk;
+
+ u64 pcr_period_usecs;
+ u64 si_period_usecs;
+};
+
+/**
+ * struct vidtv_mux_si - Store the PSI context.
+ *
+ * This is used to store the PAT, PMT sections and SDT in use by the muxer.
+ *
+ * The muxer acquire these by looking into the hardcoded channels in
+ * vidtv_channel and then periodically sends the TS packets for them>
+ *
+ * @pat: The PAT in use by the muxer.
+ * @pmt_secs: The PMT sections in use by the muxer. One for each program in the PAT.
+ * @sdt: The SDT in use by the muxer.
+ * @nit: The NIT in use by the muxer.
+ * @eit: the EIT in use by the muxer.
+ */
+struct vidtv_mux_si {
+ /* the SI tables */
+ struct vidtv_psi_table_pat *pat;
+ struct vidtv_psi_table_pmt **pmt_secs; /* the PMT sections */
+ struct vidtv_psi_table_sdt *sdt;
+ struct vidtv_psi_table_nit *nit;
+ struct vidtv_psi_table_eit *eit;
+};
+
+/**
+ * struct vidtv_mux_pid_ctx - Store the context for a given TS PID.
+ * @pid: The TS PID.
+ * @cc: The continuity counter for this PID. It is incremented on every TS
+ * pack and it will wrap around at 0xf0. If the decoder notices a sudden jump in
+ * this counter this will trigger a discontinuity state.
+ * @h: This is embedded in a hash table, mapping pid -> vidtv_mux_pid_ctx
+ */
+struct vidtv_mux_pid_ctx {
+ u16 pid;
+ u8 cc; /* continuity counter */
+ struct hlist_node h;
+};
+
+/**
+ * struct vidtv_mux - A muxer abstraction loosely based in libavcodec/mpegtsenc.c
+ * @fe: The frontend structure allocated by the muxer.
+ * @dev: pointer to struct device.
+ * @timing: Keeps track of timing related information.
+ * @mux_rate_kbytes_sec: The bit rate for the TS, in kbytes.
+ * @pid_ctx: A hash table to keep track of per-PID metadata.
+ * @on_new_packets_available_cb: A callback to inform of new TS packets ready.
+ * @mux_buf: A pointer to a buffer for this muxer. TS packets are stored there
+ * and then passed on to the bridge driver.
+ * @mux_buf_sz: The size for 'mux_buf'.
+ * @mux_buf_offset: The current offset into 'mux_buf'.
+ * @channels: The channels associated with this muxer.
+ * @si: Keeps track of the PSI context.
+ * @num_streamed_pcr: Number of PCR packets streamed.
+ * @num_streamed_si: The number of PSI packets streamed.
+ * @mpeg_thread: Thread responsible for the muxer loop.
+ * @streaming: whether 'mpeg_thread' is running.
+ * @pcr_pid: The TS PID used for the PSI packets. All channels will share the
+ * same PCR.
+ * @transport_stream_id: The transport stream ID
+ * @network_id: The network ID
+ * @network_name: The network name
+ * @priv: Private data.
+ */
+struct vidtv_mux {
+ struct dvb_frontend *fe;
+ struct device *dev;
+
+ struct vidtv_mux_timing timing;
+
+ u32 mux_rate_kbytes_sec;
+
+ DECLARE_HASHTABLE(pid_ctx, 3);
+
+ void (*on_new_packets_available_cb)(void *priv, u8 *buf, u32 npackets);
+
+ u8 *mux_buf;
+ u32 mux_buf_sz;
+ u32 mux_buf_offset;
+
+ struct vidtv_channel *channels;
+
+ struct vidtv_mux_si si;
+ u64 num_streamed_pcr;
+ u64 num_streamed_si;
+
+ struct work_struct mpeg_thread;
+ bool streaming;
+
+ u16 pcr_pid;
+ u16 transport_stream_id;
+ u16 network_id;
+ char *network_name;
+ void *priv;
+};
+
+/**
+ * struct vidtv_mux_init_args - Arguments used to inix the muxer.
+ * @mux_rate_kbytes_sec: The bit rate for the TS, in kbytes.
+ * @on_new_packets_available_cb: A callback to inform of new TS packets ready.
+ * @mux_buf_sz: The size for 'mux_buf'.
+ * @pcr_period_usecs: How often we should send PCR packets.
+ * @si_period_usecs: How often we should send PSI packets.
+ * @pcr_pid: The TS PID used for the PSI packets. All channels will share the
+ * same PCR.
+ * @transport_stream_id: The transport stream ID
+ * @channels: an optional list of channels to use
+ * @network_id: The network ID
+ * @network_name: The network name
+ * @priv: Private data.
+ */
+struct vidtv_mux_init_args {
+ u32 mux_rate_kbytes_sec;
+ void (*on_new_packets_available_cb)(void *priv, u8 *buf, u32 npackets);
+ u32 mux_buf_sz;
+ u64 pcr_period_usecs;
+ u64 si_period_usecs;
+ u16 pcr_pid;
+ u16 transport_stream_id;
+ struct vidtv_channel *channels;
+ u16 network_id;
+ char *network_name;
+ void *priv;
+};
+
+struct vidtv_mux *vidtv_mux_init(struct dvb_frontend *fe,
+ struct device *dev,
+ struct vidtv_mux_init_args *args);
+void vidtv_mux_destroy(struct vidtv_mux *m);
+
+void vidtv_mux_start_thread(struct vidtv_mux *m);
+void vidtv_mux_stop_thread(struct vidtv_mux *m);
+
+#endif //VIDTV_MUX_H
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 0000000..782e5e7
--- /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;
+}
diff --git a/drivers/media/test-drivers/vidtv/vidtv_pes.h b/drivers/media/test-drivers/vidtv/vidtv_pes.h
new file mode 100644
index 0000000..963c591
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_pes.h
@@ -0,0 +1,193 @@
+/* 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
+ */
+
+#ifndef VIDTV_PES_H
+#define VIDTV_PES_H
+
+#include <linux/types.h>
+
+#include "vidtv_common.h"
+
+#define PES_MAX_LEN 65536 /* Set 'length' to 0 if greater. Only possible for video. */
+#define PES_START_CODE_PREFIX 0x001 /* 00 00 01 */
+
+/* Used when sending PTS, but not DTS */
+struct vidtv_pes_optional_pts {
+ u8 pts1;
+ __be16 pts2;
+ __be16 pts3;
+} __packed;
+
+/* Used when sending both PTS and DTS */
+struct vidtv_pes_optional_pts_dts {
+ u8 pts1;
+ __be16 pts2;
+ __be16 pts3;
+
+ u8 dts1;
+ __be16 dts2;
+ __be16 dts3;
+} __packed;
+
+/* PES optional flags */
+struct vidtv_pes_optional {
+ /*
+ * These flags show which components are actually
+ * present in the "optional fields" in the optional PES
+ * header and which are not
+ *
+ * u16 two:2; //0x2
+ * u16 PES_scrambling_control:2;
+ * u16 PES_priority:1;
+ * u16 data_alignment_indicator:1; // unused
+ * u16 copyright:1;
+ * u16 original_or_copy:1;
+ * u16 PTS_DTS:2;
+ * u16 ESCR:1;
+ * u16 ES_rate:1;
+ * u16 DSM_trick_mode:1;
+ * u16 additional_copy_info:1;
+ * u16 PES_CRC:1;
+ * u16 PES_extension:1;
+ */
+ __be16 bitfield;
+ u8 length;
+} __packed;
+
+/* The PES header */
+struct vidtv_mpeg_pes {
+ __be32 bitfield; /* packet_start_code_prefix:24, stream_id: 8 */
+ /* after this field until the end of the PES data payload */
+ __be16 length;
+ struct vidtv_pes_optional optional[];
+} __packed;
+
+/**
+ * struct pes_header_write_args - Arguments to write a PES header.
+ * @dest_buf: The buffer to write into.
+ * @dest_offset: where to start writing in the dest_buffer.
+ * @dest_buf_sz: The size of the dest_buffer
+ * @encoder_id: Encoder id (see vidtv_encoder.h)
+ * @send_pts: Should we send PTS?
+ * @pts: PTS value to send.
+ * @send_dts: Should we send DTS?
+ * @dts: DTS value to send.
+ * @stream_id: The stream id to use. Ex: Audio streams (0xc0-0xdf), Video
+ * streams (0xe0-0xef).
+ * @n_pes_h_s_bytes: Padding bytes. Might be used by an encoder if needed, gets
+ * discarded by the decoder.
+ * @access_unit_len: The size of _one_ access unit (with any headers it might need)
+ */
+struct pes_header_write_args {
+ void *dest_buf;
+ u32 dest_offset;
+ u32 dest_buf_sz;
+ u32 encoder_id;
+
+ bool send_pts;
+ u64 pts;
+
+ bool send_dts;
+ u64 dts;
+
+ u16 stream_id;
+ /* might be used by an encoder if needed, gets discarded by decoder */
+ u32 n_pes_h_s_bytes;
+ u32 access_unit_len;
+};
+
+/**
+ * struct pes_ts_header_write_args - Arguments to write a TS header.
+ * @dest_buf: The buffer to write into.
+ * @dest_offset: where to start writing in the dest_buffer.
+ * @dest_buf_sz: The size of the dest_buffer
+ * @pid: The PID to use for the TS packets.
+ * @continuity_counter: Incremented on every new TS packet.
+ * @wrote_pes_header: Flag to indicate that the PES header was written
+ * @n_stuffing_bytes: Padding bytes. Might be used by an encoder if needed, gets
+ * discarded by the decoder.
+ * @pcr: counter driven by a 27Mhz clock.
+ */
+struct pes_ts_header_write_args {
+ void *dest_buf;
+ u32 dest_offset;
+ u32 dest_buf_sz;
+ u16 pid;
+ u8 *continuity_counter;
+ bool wrote_pes_header;
+ u32 n_stuffing_bytes;
+ u64 pcr;
+};
+
+/**
+ * struct pes_write_args - Arguments for the packetizer.
+ * @dest_buf: The buffer to write into.
+ * @from: A pointer to the encoder buffer containing one access unit.
+ * @access_unit_len: The size of _one_ access unit (with any headers it might need)
+ * @dest_offset: where to start writing in the dest_buffer.
+ * @dest_buf_sz: The size of the dest_buffer
+ * @pid: The PID to use for the TS packets.
+ * @encoder_id: Encoder id (see vidtv_encoder.h)
+ * @continuity_counter: Incremented on every new TS packet.
+ * @stream_id: The stream id to use. Ex: Audio streams (0xc0-0xdf), Video
+ * streams (0xe0-0xef).
+ * @send_pts: Should we send PTS?
+ * @pts: PTS value to send.
+ * @send_dts: Should we send DTS?
+ * @dts: DTS value to send.
+ * @n_pes_h_s_bytes: Padding bytes. Might be used by an encoder if needed, gets
+ * discarded by the decoder.
+ * @pcr: counter driven by a 27Mhz clock.
+ */
+struct pes_write_args {
+ void *dest_buf;
+ void *from;
+ u32 access_unit_len;
+
+ u32 dest_offset;
+ u32 dest_buf_sz;
+ u16 pid;
+
+ u32 encoder_id;
+
+ u8 *continuity_counter;
+
+ u16 stream_id;
+
+ bool send_pts;
+ u64 pts;
+
+ bool send_dts;
+ u64 dts;
+
+ u32 n_pes_h_s_bytes;
+ u64 pcr;
+};
+
+/**
+ * vidtv_pes_write_into - Write a PES packet as MPEG-TS packets into a buffer.
+ * @args: The args to use when writing
+ *
+ * This function 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.
+ *
+ * The data is then written into the buffer pointed to by 'args.buf'
+ *
+ * Return: The number of bytes written into the buffer. This is usually NOT
+ * equal to the size of the access unit, since we need space for PES headers, TS headers
+ * and padding bytes, if any.
+ */
+u32 vidtv_pes_write_into(struct pes_write_args *args);
+
+#endif // VIDTV_PES_H
diff --git a/drivers/media/test-drivers/vidtv/vidtv_psi.c b/drivers/media/test-drivers/vidtv/vidtv_psi.c
new file mode 100644
index 0000000..1724bb4
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_psi.c
@@ -0,0 +1,2026 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * This file contains the logic to work with MPEG Program-Specific Information.
+ * These are defined both in ISO/IEC 13818-1 (systems) and ETSI EN 300 468.
+ * PSI is carried in the form of table structures, and although each table might
+ * technically be broken into one or more sections, we do not do this here,
+ * hence 'table' and 'section' are interchangeable for vidtv.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ":%s, %d: " fmt, __func__, __LINE__
+
+#include <linux/bcd.h>
+#include <linux/crc32.h>
+#include <linux/kernel.h>
+#include <linux/ktime.h>
+#include <linux/printk.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/string.h>
+#include <linux/time.h>
+#include <linux/types.h>
+
+#include "vidtv_common.h"
+#include "vidtv_psi.h"
+#include "vidtv_ts.h"
+
+#define CRC_SIZE_IN_BYTES 4
+#define MAX_VERSION_NUM 32
+#define INITIAL_CRC 0xffffffff
+#define ISO_LANGUAGE_CODE_LEN 3
+
+static const u32 CRC_LUT[256] = {
+ /* from libdvbv5 */
+ 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b,
+ 0x1a864db2, 0x1e475005, 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61,
+ 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, 0x4c11db70, 0x48d0c6c7,
+ 0x4593e01e, 0x4152fda9, 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75,
+ 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3,
+ 0x709f7b7a, 0x745e66cd, 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039,
+ 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, 0xbe2b5b58, 0xbaea46ef,
+ 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d,
+ 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, 0xc7361b4c, 0xc3f706fb,
+ 0xceb42022, 0xca753d95, 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1,
+ 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, 0x34867077, 0x30476dc0,
+ 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072,
+ 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, 0x018aeb13, 0x054bf6a4,
+ 0x0808d07d, 0x0cc9cdca, 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde,
+ 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, 0x5e9f46bf, 0x5a5e5b08,
+ 0x571d7dd1, 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba,
+ 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, 0xbb60adfc,
+ 0xb6238b25, 0xb2e29692, 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6,
+ 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, 0xe0b41de7, 0xe4750050,
+ 0xe9362689, 0xedf73b3e, 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2,
+ 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34,
+ 0xdc3abded, 0xd8fba05a, 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637,
+ 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, 0x4f040d56, 0x4bc510e1,
+ 0x46863638, 0x42472b8f, 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53,
+ 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5,
+ 0x3f9b762c, 0x3b5a6b9b, 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff,
+ 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, 0xf12f560e, 0xf5ee4bb9,
+ 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b,
+ 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd,
+ 0xcda1f604, 0xc960ebb3, 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7,
+ 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, 0x9b3660c6, 0x9ff77d71,
+ 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3,
+ 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, 0x4e8ee645, 0x4a4ffbf2,
+ 0x470cdd2b, 0x43cdc09c, 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8,
+ 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, 0x119b4be9, 0x155a565e,
+ 0x18197087, 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec,
+ 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, 0x2056cd3a,
+ 0x2d15ebe3, 0x29d4f654, 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0,
+ 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, 0xe3a1cbc1, 0xe760d676,
+ 0xea23f0af, 0xeee2ed18, 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4,
+ 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662,
+ 0x933eb0bb, 0x97ffad0c, 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668,
+ 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4
+};
+
+static u32 dvb_crc32(u32 crc, u8 *data, u32 len)
+{
+ /* from libdvbv5 */
+ while (len--)
+ crc = (crc << 8) ^ CRC_LUT[((crc >> 24) ^ *data++) & 0xff];
+ return crc;
+}
+
+static void vidtv_psi_update_version_num(struct vidtv_psi_table_header *h)
+{
+ h->version++;
+}
+
+static u16 vidtv_psi_get_sec_len(struct vidtv_psi_table_header *h)
+{
+ u16 mask;
+ u16 ret;
+
+ mask = GENMASK(11, 0);
+
+ ret = be16_to_cpu(h->bitfield) & mask;
+ return ret;
+}
+
+u16 vidtv_psi_get_pat_program_pid(struct vidtv_psi_table_pat_program *p)
+{
+ u16 mask;
+ u16 ret;
+
+ mask = GENMASK(12, 0);
+
+ ret = be16_to_cpu(p->bitfield) & mask;
+ return ret;
+}
+
+u16 vidtv_psi_pmt_stream_get_elem_pid(struct vidtv_psi_table_pmt_stream *s)
+{
+ u16 mask;
+ u16 ret;
+
+ mask = GENMASK(12, 0);
+
+ ret = be16_to_cpu(s->bitfield) & mask;
+ return ret;
+}
+
+static void vidtv_psi_set_desc_loop_len(__be16 *bitfield, u16 new_len,
+ u8 desc_len_nbits)
+{
+ __be16 new;
+ u16 mask;
+
+ mask = GENMASK(15, desc_len_nbits);
+
+ new = cpu_to_be16((be16_to_cpu(*bitfield) & mask) | new_len);
+ *bitfield = new;
+}
+
+static void vidtv_psi_set_sec_len(struct vidtv_psi_table_header *h, u16 new_len)
+{
+ u16 old_len = vidtv_psi_get_sec_len(h);
+ __be16 new;
+ u16 mask;
+
+ mask = GENMASK(15, 13);
+
+ new = cpu_to_be16((be16_to_cpu(h->bitfield) & mask) | new_len);
+
+ if (old_len > MAX_SECTION_LEN)
+ pr_warn_ratelimited("section length: %d > %d, old len was %d\n",
+ new_len,
+ MAX_SECTION_LEN,
+ old_len);
+
+ h->bitfield = new;
+}
+
+/*
+ * Packetize PSI sections into TS packets:
+ * push a TS header (4bytes) every 184 bytes
+ * manage the continuity_counter
+ * add stuffing (i.e. padding bytes) after the CRC
+ */
+static u32 vidtv_psi_ts_psi_write_into(struct psi_write_args *args)
+{
+ struct vidtv_mpeg_ts ts_header = {
+ .sync_byte = TS_SYNC_BYTE,
+ .bitfield = cpu_to_be16((args->new_psi_section << 14) | args->pid),
+ .scrambling = 0,
+ .payload = 1,
+ .adaptation_field = 0, /* no adaptation field */
+ };
+ u32 nbytes_past_boundary = (args->dest_offset % TS_PACKET_LEN);
+ bool aligned = (nbytes_past_boundary == 0);
+ u32 remaining_len = args->len;
+ u32 payload_write_len = 0;
+ u32 payload_offset = 0;
+ u32 nbytes = 0;
+
+ if (!args->crc && !args->is_crc)
+ pr_warn_ratelimited("Missing CRC for chunk\n");
+
+ if (args->crc)
+ *args->crc = dvb_crc32(*args->crc, args->from, args->len);
+
+ if (args->new_psi_section && !aligned) {
+ pr_warn_ratelimited("Cannot write a new PSI section in a misaligned buffer\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 - nbytes_past_boundary);
+ }
+
+ while (remaining_len) {
+ nbytes_past_boundary = (args->dest_offset + nbytes) % TS_PACKET_LEN;
+ aligned = (nbytes_past_boundary == 0);
+
+ if (aligned) {
+ /* if at a packet boundary, write a new TS header */
+ ts_header.continuity_counter = *args->continuity_counter;
+
+ nbytes += vidtv_memcpy(args->dest_buf,
+ args->dest_offset + nbytes,
+ args->dest_buf_sz,
+ &ts_header,
+ sizeof(ts_header));
+ /*
+ * This will trigger a discontinuity if the buffer is full,
+ * effectively dropping the packet.
+ */
+ vidtv_ts_inc_cc(args->continuity_counter);
+ }
+
+ /* write the pointer_field in the first byte of the payload */
+ if (args->new_psi_section)
+ nbytes += vidtv_memset(args->dest_buf,
+ args->dest_offset + nbytes,
+ args->dest_buf_sz,
+ 0x0,
+ 1);
+
+ /* write as much of the payload as possible */
+ nbytes_past_boundary = (args->dest_offset + nbytes) % TS_PACKET_LEN;
+ payload_write_len = min(TS_PACKET_LEN - nbytes_past_boundary, remaining_len);
+
+ nbytes += vidtv_memcpy(args->dest_buf,
+ args->dest_offset + nbytes,
+ args->dest_buf_sz,
+ args->from + payload_offset,
+ payload_write_len);
+
+ /* 'payload_write_len' written from a total of 'len' requested*/
+ remaining_len -= payload_write_len;
+ payload_offset += payload_write_len;
+ }
+
+ /*
+ * fill the rest of the packet if there is any remaining space unused
+ */
+
+ nbytes_past_boundary = (args->dest_offset + nbytes) % TS_PACKET_LEN;
+
+ if (args->is_crc)
+ nbytes += vidtv_memset(args->dest_buf,
+ args->dest_offset + nbytes,
+ args->dest_buf_sz,
+ TS_FILL_BYTE,
+ TS_PACKET_LEN - nbytes_past_boundary);
+
+ return nbytes;
+}
+
+static u32 table_section_crc32_write_into(struct crc32_write_args *args)
+{
+ struct psi_write_args psi_args = {
+ .dest_buf = args->dest_buf,
+ .from = &args->crc,
+ .len = CRC_SIZE_IN_BYTES,
+ .dest_offset = args->dest_offset,
+ .pid = args->pid,
+ .new_psi_section = false,
+ .continuity_counter = args->continuity_counter,
+ .is_crc = true,
+ .dest_buf_sz = args->dest_buf_sz,
+ };
+
+ /* the CRC is the last entry in the section */
+
+ return vidtv_psi_ts_psi_write_into(&psi_args);
+}
+
+static void vidtv_psi_desc_chain(struct vidtv_psi_desc *head, struct vidtv_psi_desc *desc)
+{
+ if (head) {
+ while (head->next)
+ head = head->next;
+
+ head->next = desc;
+ }
+}
+
+struct vidtv_psi_desc_service *vidtv_psi_service_desc_init(struct vidtv_psi_desc *head,
+ enum service_type service_type,
+ char *service_name,
+ char *provider_name)
+{
+ struct vidtv_psi_desc_service *desc;
+ u32 service_name_len = service_name ? strlen(service_name) : 0;
+ u32 provider_name_len = provider_name ? strlen(provider_name) : 0;
+
+ desc = kzalloc(sizeof(*desc), GFP_KERNEL);
+ if (!desc)
+ return NULL;
+
+ desc->type = SERVICE_DESCRIPTOR;
+
+ desc->length = sizeof_field(struct vidtv_psi_desc_service, service_type)
+ + sizeof_field(struct vidtv_psi_desc_service, provider_name_len)
+ + provider_name_len
+ + sizeof_field(struct vidtv_psi_desc_service, service_name_len)
+ + service_name_len;
+
+ desc->service_type = service_type;
+
+ desc->service_name_len = service_name_len;
+
+ if (service_name && service_name_len)
+ desc->service_name = kstrdup(service_name, GFP_KERNEL);
+
+ desc->provider_name_len = provider_name_len;
+
+ if (provider_name && provider_name_len)
+ desc->provider_name = kstrdup(provider_name, GFP_KERNEL);
+
+ vidtv_psi_desc_chain(head, (struct vidtv_psi_desc *)desc);
+ return desc;
+}
+
+struct vidtv_psi_desc_registration
+*vidtv_psi_registration_desc_init(struct vidtv_psi_desc *head,
+ __be32 format_id,
+ u8 *additional_ident_info,
+ u32 additional_info_len)
+{
+ struct vidtv_psi_desc_registration *desc;
+
+ desc = kzalloc(sizeof(*desc) + sizeof(format_id) + additional_info_len, GFP_KERNEL);
+ if (!desc)
+ return NULL;
+
+ desc->type = REGISTRATION_DESCRIPTOR;
+
+ desc->length = sizeof_field(struct vidtv_psi_desc_registration, format_id)
+ + additional_info_len;
+
+ desc->format_id = format_id;
+
+ if (additional_ident_info && additional_info_len)
+ memcpy(desc->additional_identification_info,
+ additional_ident_info,
+ additional_info_len);
+
+ vidtv_psi_desc_chain(head, (struct vidtv_psi_desc *)desc);
+ return desc;
+}
+
+struct vidtv_psi_desc_network_name
+*vidtv_psi_network_name_desc_init(struct vidtv_psi_desc *head, char *network_name)
+{
+ u32 network_name_len = network_name ? strlen(network_name) : 0;
+ struct vidtv_psi_desc_network_name *desc;
+
+ desc = kzalloc(sizeof(*desc), GFP_KERNEL);
+ if (!desc)
+ return NULL;
+
+ desc->type = NETWORK_NAME_DESCRIPTOR;
+
+ desc->length = network_name_len;
+
+ if (network_name && network_name_len)
+ desc->network_name = kstrdup(network_name, GFP_KERNEL);
+
+ vidtv_psi_desc_chain(head, (struct vidtv_psi_desc *)desc);
+ return desc;
+}
+
+struct vidtv_psi_desc_service_list
+*vidtv_psi_service_list_desc_init(struct vidtv_psi_desc *head,
+ struct vidtv_psi_desc_service_list_entry *entry)
+{
+ struct vidtv_psi_desc_service_list_entry *curr_e = NULL;
+ struct vidtv_psi_desc_service_list_entry *head_e = NULL;
+ struct vidtv_psi_desc_service_list_entry *prev_e = NULL;
+ struct vidtv_psi_desc_service_list *desc;
+ u16 length = 0;
+
+ desc = kzalloc(sizeof(*desc), GFP_KERNEL);
+ if (!desc)
+ return NULL;
+
+ desc->type = SERVICE_LIST_DESCRIPTOR;
+
+ while (entry) {
+ curr_e = kzalloc(sizeof(*curr_e), GFP_KERNEL);
+ if (!curr_e) {
+ while (head_e) {
+ curr_e = head_e;
+ head_e = head_e->next;
+ kfree(curr_e);
+ }
+ kfree(desc);
+ return NULL;
+ }
+
+ curr_e->service_id = entry->service_id;
+ curr_e->service_type = entry->service_type;
+
+ length += sizeof(struct vidtv_psi_desc_service_list_entry) -
+ sizeof(struct vidtv_psi_desc_service_list_entry *);
+
+ if (!head_e)
+ head_e = curr_e;
+ if (prev_e)
+ prev_e->next = curr_e;
+
+ prev_e = curr_e;
+ entry = entry->next;
+ }
+
+ desc->length = length;
+ desc->service_list = head_e;
+
+ vidtv_psi_desc_chain(head, (struct vidtv_psi_desc *)desc);
+ return desc;
+}
+
+struct vidtv_psi_desc_short_event
+*vidtv_psi_short_event_desc_init(struct vidtv_psi_desc *head,
+ char *iso_language_code,
+ char *event_name,
+ char *text)
+{
+ u32 iso_len = iso_language_code ? strlen(iso_language_code) : 0;
+ u32 event_name_len = event_name ? strlen(event_name) : 0;
+ struct vidtv_psi_desc_short_event *desc;
+ u32 text_len = text ? strlen(text) : 0;
+
+ desc = kzalloc(sizeof(*desc), GFP_KERNEL);
+ if (!desc)
+ return NULL;
+
+ desc->type = SHORT_EVENT_DESCRIPTOR;
+
+ desc->length = ISO_LANGUAGE_CODE_LEN +
+ sizeof_field(struct vidtv_psi_desc_short_event, event_name_len) +
+ event_name_len +
+ sizeof_field(struct vidtv_psi_desc_short_event, text_len) +
+ text_len;
+
+ desc->event_name_len = event_name_len;
+ desc->text_len = text_len;
+
+ if (iso_len != ISO_LANGUAGE_CODE_LEN)
+ iso_language_code = "eng";
+
+ desc->iso_language_code = kstrdup(iso_language_code, GFP_KERNEL);
+
+ if (event_name && event_name_len)
+ desc->event_name = kstrdup(event_name, GFP_KERNEL);
+
+ if (text && text_len)
+ desc->text = kstrdup(text, GFP_KERNEL);
+
+ vidtv_psi_desc_chain(head, (struct vidtv_psi_desc *)desc);
+ return desc;
+}
+
+struct vidtv_psi_desc *vidtv_psi_desc_clone(struct vidtv_psi_desc *desc)
+{
+ struct vidtv_psi_desc_network_name *desc_network_name;
+ struct vidtv_psi_desc_service_list *desc_service_list;
+ struct vidtv_psi_desc_short_event *desc_short_event;
+ struct vidtv_psi_desc_service *service;
+ struct vidtv_psi_desc *head = NULL;
+ struct vidtv_psi_desc *prev = NULL;
+ struct vidtv_psi_desc *curr = NULL;
+
+ while (desc) {
+ switch (desc->type) {
+ case SERVICE_DESCRIPTOR:
+ service = (struct vidtv_psi_desc_service *)desc;
+ curr = (struct vidtv_psi_desc *)
+ vidtv_psi_service_desc_init(head,
+ service->service_type,
+ service->service_name,
+ service->provider_name);
+ break;
+
+ case NETWORK_NAME_DESCRIPTOR:
+ desc_network_name = (struct vidtv_psi_desc_network_name *)desc;
+ curr = (struct vidtv_psi_desc *)
+ vidtv_psi_network_name_desc_init(head,
+ desc_network_name->network_name);
+ break;
+
+ case SERVICE_LIST_DESCRIPTOR:
+ desc_service_list = (struct vidtv_psi_desc_service_list *)desc;
+ curr = (struct vidtv_psi_desc *)
+ vidtv_psi_service_list_desc_init(head,
+ desc_service_list->service_list);
+ break;
+
+ case SHORT_EVENT_DESCRIPTOR:
+ desc_short_event = (struct vidtv_psi_desc_short_event *)desc;
+ curr = (struct vidtv_psi_desc *)
+ vidtv_psi_short_event_desc_init(head,
+ desc_short_event->iso_language_code,
+ desc_short_event->event_name,
+ desc_short_event->text);
+ break;
+
+ case REGISTRATION_DESCRIPTOR:
+ default:
+ curr = kzalloc(sizeof(*desc) + desc->length, GFP_KERNEL);
+ if (!curr)
+ return NULL;
+ memcpy(curr, desc, sizeof(*desc) + desc->length);
+ }
+
+ if (!curr)
+ return NULL;
+
+ curr->next = NULL;
+ if (!head)
+ head = curr;
+ if (prev)
+ prev->next = curr;
+
+ prev = curr;
+ desc = desc->next;
+ }
+
+ return head;
+}
+
+void vidtv_psi_desc_destroy(struct vidtv_psi_desc *desc)
+{
+ struct vidtv_psi_desc_service_list_entry *sl_entry_tmp = NULL;
+ struct vidtv_psi_desc_service_list_entry *sl_entry = NULL;
+ struct vidtv_psi_desc *curr = desc;
+ struct vidtv_psi_desc *tmp = NULL;
+
+ while (curr) {
+ tmp = curr;
+ curr = curr->next;
+
+ switch (tmp->type) {
+ case SERVICE_DESCRIPTOR:
+ kfree(((struct vidtv_psi_desc_service *)tmp)->provider_name);
+ kfree(((struct vidtv_psi_desc_service *)tmp)->service_name);
+
+ break;
+ case REGISTRATION_DESCRIPTOR:
+ /* nothing to do */
+ break;
+
+ case NETWORK_NAME_DESCRIPTOR:
+ kfree(((struct vidtv_psi_desc_network_name *)tmp)->network_name);
+ break;
+
+ case SERVICE_LIST_DESCRIPTOR:
+ sl_entry = ((struct vidtv_psi_desc_service_list *)tmp)->service_list;
+ while (sl_entry) {
+ sl_entry_tmp = sl_entry;
+ sl_entry = sl_entry->next;
+ kfree(sl_entry_tmp);
+ }
+ break;
+
+ case SHORT_EVENT_DESCRIPTOR:
+ kfree(((struct vidtv_psi_desc_short_event *)tmp)->iso_language_code);
+ kfree(((struct vidtv_psi_desc_short_event *)tmp)->event_name);
+ kfree(((struct vidtv_psi_desc_short_event *)tmp)->text);
+ break;
+
+ default:
+ pr_warn_ratelimited("Possible leak: not handling descriptor type %d\n",
+ tmp->type);
+ break;
+ }
+
+ kfree(tmp);
+ }
+}
+
+static u16
+vidtv_psi_desc_comp_loop_len(struct vidtv_psi_desc *desc)
+{
+ u32 length = 0;
+
+ if (!desc)
+ return 0;
+
+ while (desc) {
+ length += sizeof_field(struct vidtv_psi_desc, type);
+ length += sizeof_field(struct vidtv_psi_desc, length);
+ length += desc->length; /* from 'length' field until the end of the descriptor */
+ desc = desc->next;
+ }
+
+ return length;
+}
+
+void vidtv_psi_desc_assign(struct vidtv_psi_desc **to,
+ struct vidtv_psi_desc *desc)
+{
+ if (desc == *to)
+ return;
+
+ if (*to)
+ vidtv_psi_desc_destroy(*to);
+
+ *to = desc;
+}
+
+void vidtv_pmt_desc_assign(struct vidtv_psi_table_pmt *pmt,
+ struct vidtv_psi_desc **to,
+ struct vidtv_psi_desc *desc)
+{
+ vidtv_psi_desc_assign(to, desc);
+ vidtv_psi_pmt_table_update_sec_len(pmt);
+
+ if (vidtv_psi_get_sec_len(&pmt->header) > MAX_SECTION_LEN)
+ vidtv_psi_desc_assign(to, NULL);
+
+ vidtv_psi_update_version_num(&pmt->header);
+}
+
+void vidtv_sdt_desc_assign(struct vidtv_psi_table_sdt *sdt,
+ struct vidtv_psi_desc **to,
+ struct vidtv_psi_desc *desc)
+{
+ vidtv_psi_desc_assign(to, desc);
+ vidtv_psi_sdt_table_update_sec_len(sdt);
+
+ if (vidtv_psi_get_sec_len(&sdt->header) > MAX_SECTION_LEN)
+ vidtv_psi_desc_assign(to, NULL);
+
+ vidtv_psi_update_version_num(&sdt->header);
+}
+
+static u32 vidtv_psi_desc_write_into(struct desc_write_args *args)
+{
+ struct psi_write_args psi_args = {
+ .dest_buf = args->dest_buf,
+ .from = &args->desc->type,
+ .pid = args->pid,
+ .new_psi_section = false,
+ .continuity_counter = args->continuity_counter,
+ .is_crc = false,
+ .dest_buf_sz = args->dest_buf_sz,
+ .crc = args->crc,
+ .len = sizeof_field(struct vidtv_psi_desc, type) +
+ sizeof_field(struct vidtv_psi_desc, length),
+ };
+ struct vidtv_psi_desc_service_list_entry *serv_list_entry = NULL;
+ u32 nbytes = 0;
+
+ psi_args.dest_offset = args->dest_offset + nbytes;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ switch (args->desc->type) {
+ case SERVICE_DESCRIPTOR:
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = sizeof_field(struct vidtv_psi_desc_service, service_type) +
+ sizeof_field(struct vidtv_psi_desc_service, provider_name_len);
+ psi_args.from = &((struct vidtv_psi_desc_service *)args->desc)->service_type;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = ((struct vidtv_psi_desc_service *)args->desc)->provider_name_len;
+ psi_args.from = ((struct vidtv_psi_desc_service *)args->desc)->provider_name;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = sizeof_field(struct vidtv_psi_desc_service, service_name_len);
+ psi_args.from = &((struct vidtv_psi_desc_service *)args->desc)->service_name_len;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = ((struct vidtv_psi_desc_service *)args->desc)->service_name_len;
+ psi_args.from = ((struct vidtv_psi_desc_service *)args->desc)->service_name;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+ break;
+
+ case NETWORK_NAME_DESCRIPTOR:
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = args->desc->length;
+ psi_args.from = ((struct vidtv_psi_desc_network_name *)args->desc)->network_name;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+ break;
+
+ case SERVICE_LIST_DESCRIPTOR:
+ serv_list_entry = ((struct vidtv_psi_desc_service_list *)args->desc)->service_list;
+ while (serv_list_entry) {
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = sizeof(struct vidtv_psi_desc_service_list_entry) -
+ sizeof(struct vidtv_psi_desc_service_list_entry *);
+ psi_args.from = serv_list_entry;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ serv_list_entry = serv_list_entry->next;
+ }
+ break;
+
+ case SHORT_EVENT_DESCRIPTOR:
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = ISO_LANGUAGE_CODE_LEN;
+ psi_args.from = ((struct vidtv_psi_desc_short_event *)
+ args->desc)->iso_language_code;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = sizeof_field(struct vidtv_psi_desc_short_event, event_name_len);
+ psi_args.from = &((struct vidtv_psi_desc_short_event *)
+ args->desc)->event_name_len;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = ((struct vidtv_psi_desc_short_event *)args->desc)->event_name_len;
+ psi_args.from = ((struct vidtv_psi_desc_short_event *)args->desc)->event_name;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = sizeof_field(struct vidtv_psi_desc_short_event, text_len);
+ psi_args.from = &((struct vidtv_psi_desc_short_event *)args->desc)->text_len;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = ((struct vidtv_psi_desc_short_event *)args->desc)->text_len;
+ psi_args.from = ((struct vidtv_psi_desc_short_event *)args->desc)->text;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ break;
+
+ case REGISTRATION_DESCRIPTOR:
+ default:
+ psi_args.dest_offset = args->dest_offset + nbytes;
+ psi_args.len = args->desc->length;
+ psi_args.from = &args->desc->data;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+ break;
+ }
+
+ return nbytes;
+}
+
+static u32
+vidtv_psi_table_header_write_into(struct header_write_args *args)
+{
+ struct psi_write_args psi_args = {
+ .dest_buf = args->dest_buf,
+ .from = args->h,
+ .len = sizeof(struct vidtv_psi_table_header),
+ .dest_offset = args->dest_offset,
+ .pid = args->pid,
+ .new_psi_section = true,
+ .continuity_counter = args->continuity_counter,
+ .is_crc = false,
+ .dest_buf_sz = args->dest_buf_sz,
+ .crc = args->crc,
+ };
+
+ return vidtv_psi_ts_psi_write_into(&psi_args);
+}
+
+void
+vidtv_psi_pat_table_update_sec_len(struct vidtv_psi_table_pat *pat)
+{
+ u16 length = 0;
+ u32 i;
+
+ /* see ISO/IEC 13818-1 : 2000 p.43 */
+
+ /* from immediately after 'section_length' until 'last_section_number'*/
+ length += PAT_LEN_UNTIL_LAST_SECTION_NUMBER;
+
+ /* do not count the pointer */
+ for (i = 0; i < pat->num_pat; ++i)
+ length += sizeof(struct vidtv_psi_table_pat_program) -
+ sizeof(struct vidtv_psi_table_pat_program *);
+
+ length += CRC_SIZE_IN_BYTES;
+
+ vidtv_psi_set_sec_len(&pat->header, length);
+}
+
+void vidtv_psi_pmt_table_update_sec_len(struct vidtv_psi_table_pmt *pmt)
+{
+ struct vidtv_psi_table_pmt_stream *s = pmt->stream;
+ u16 desc_loop_len;
+ u16 length = 0;
+
+ /* see ISO/IEC 13818-1 : 2000 p.46 */
+
+ /* from immediately after 'section_length' until 'program_info_length'*/
+ length += PMT_LEN_UNTIL_PROGRAM_INFO_LENGTH;
+
+ desc_loop_len = vidtv_psi_desc_comp_loop_len(pmt->descriptor);
+ vidtv_psi_set_desc_loop_len(&pmt->bitfield2, desc_loop_len, 10);
+
+ length += desc_loop_len;
+
+ while (s) {
+ /* skip both pointers at the end */
+ length += sizeof(struct vidtv_psi_table_pmt_stream) -
+ sizeof(struct vidtv_psi_desc *) -
+ sizeof(struct vidtv_psi_table_pmt_stream *);
+
+ desc_loop_len = vidtv_psi_desc_comp_loop_len(s->descriptor);
+ vidtv_psi_set_desc_loop_len(&s->bitfield2, desc_loop_len, 10);
+
+ length += desc_loop_len;
+
+ s = s->next;
+ }
+
+ length += CRC_SIZE_IN_BYTES;
+
+ vidtv_psi_set_sec_len(&pmt->header, length);
+}
+
+void vidtv_psi_sdt_table_update_sec_len(struct vidtv_psi_table_sdt *sdt)
+{
+ struct vidtv_psi_table_sdt_service *s = sdt->service;
+ u16 desc_loop_len;
+ u16 length = 0;
+
+ /* see ETSI EN 300 468 V 1.10.1 p.24 */
+
+ /*
+ * from immediately after 'section_length' until
+ * 'reserved_for_future_use'
+ */
+ length += SDT_LEN_UNTIL_RESERVED_FOR_FUTURE_USE;
+
+ while (s) {
+ /* skip both pointers at the end */
+ length += sizeof(struct vidtv_psi_table_sdt_service) -
+ sizeof(struct vidtv_psi_desc *) -
+ sizeof(struct vidtv_psi_table_sdt_service *);
+
+ desc_loop_len = vidtv_psi_desc_comp_loop_len(s->descriptor);
+ vidtv_psi_set_desc_loop_len(&s->bitfield, desc_loop_len, 12);
+
+ length += desc_loop_len;
+
+ s = s->next;
+ }
+
+ length += CRC_SIZE_IN_BYTES;
+ vidtv_psi_set_sec_len(&sdt->header, length);
+}
+
+struct vidtv_psi_table_pat_program*
+vidtv_psi_pat_program_init(struct vidtv_psi_table_pat_program *head,
+ u16 service_id,
+ u16 program_map_pid)
+{
+ struct vidtv_psi_table_pat_program *program;
+ const u16 RESERVED = 0x07;
+
+ program = kzalloc(sizeof(*program), GFP_KERNEL);
+ if (!program)
+ return NULL;
+
+ program->service_id = cpu_to_be16(service_id);
+
+ /* pid for the PMT section in the TS */
+ program->bitfield = cpu_to_be16((RESERVED << 13) | program_map_pid);
+ program->next = NULL;
+
+ if (head) {
+ while (head->next)
+ head = head->next;
+
+ head->next = program;
+ }
+
+ return program;
+}
+
+void
+vidtv_psi_pat_program_destroy(struct vidtv_psi_table_pat_program *p)
+{
+ struct vidtv_psi_table_pat_program *tmp = NULL;
+ struct vidtv_psi_table_pat_program *curr = p;
+
+ while (curr) {
+ tmp = curr;
+ curr = curr->next;
+ kfree(tmp);
+ }
+}
+
+/* This function transfers ownership of p to the table */
+void
+vidtv_psi_pat_program_assign(struct vidtv_psi_table_pat *pat,
+ struct vidtv_psi_table_pat_program *p)
+{
+ struct vidtv_psi_table_pat_program *program;
+ u16 program_count;
+
+ do {
+ program_count = 0;
+ program = p;
+
+ if (p == pat->program)
+ return;
+
+ while (program) {
+ ++program_count;
+ program = program->next;
+ }
+
+ pat->num_pat = program_count;
+ pat->program = p;
+
+ /* Recompute section length */
+ vidtv_psi_pat_table_update_sec_len(pat);
+
+ p = NULL;
+ } while (vidtv_psi_get_sec_len(&pat->header) > MAX_SECTION_LEN);
+
+ vidtv_psi_update_version_num(&pat->header);
+}
+
+struct vidtv_psi_table_pat *vidtv_psi_pat_table_init(u16 transport_stream_id)
+{
+ struct vidtv_psi_table_pat *pat;
+ const u16 SYNTAX = 0x1;
+ const u16 ZERO = 0x0;
+ const u16 ONES = 0x03;
+
+ pat = kzalloc(sizeof(*pat), GFP_KERNEL);
+ if (!pat)
+ return NULL;
+
+ pat->header.table_id = 0x0;
+
+ pat->header.bitfield = cpu_to_be16((SYNTAX << 15) | (ZERO << 14) | (ONES << 12));
+ pat->header.id = cpu_to_be16(transport_stream_id);
+ pat->header.current_next = 0x1;
+
+ pat->header.version = 0x1f;
+
+ pat->header.one2 = 0x03;
+ pat->header.section_id = 0x0;
+ pat->header.last_section = 0x0;
+
+ vidtv_psi_pat_table_update_sec_len(pat);
+
+ return pat;
+}
+
+u32 vidtv_psi_pat_write_into(struct vidtv_psi_pat_write_args *args)
+{
+ struct vidtv_psi_table_pat_program *p = args->pat->program;
+ struct header_write_args h_args = {
+ .dest_buf = args->buf,
+ .dest_offset = args->offset,
+ .pid = VIDTV_PAT_PID,
+ .h = &args->pat->header,
+ .continuity_counter = args->continuity_counter,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct psi_write_args psi_args = {
+ .dest_buf = args->buf,
+ .pid = VIDTV_PAT_PID,
+ .new_psi_section = false,
+ .continuity_counter = args->continuity_counter,
+ .is_crc = false,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct crc32_write_args c_args = {
+ .dest_buf = args->buf,
+ .pid = VIDTV_PAT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ u32 crc = INITIAL_CRC;
+ u32 nbytes = 0;
+
+ vidtv_psi_pat_table_update_sec_len(args->pat);
+
+ h_args.crc = &crc;
+
+ nbytes += vidtv_psi_table_header_write_into(&h_args);
+
+ /* note that the field 'u16 programs' is not really part of the PAT */
+
+ psi_args.crc = &crc;
+
+ while (p) {
+ /* copy the PAT programs */
+ psi_args.from = p;
+ /* skip the pointer */
+ psi_args.len = sizeof(*p) -
+ sizeof(struct vidtv_psi_table_pat_program *);
+ psi_args.dest_offset = args->offset + nbytes;
+ psi_args.continuity_counter = args->continuity_counter;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ p = p->next;
+ }
+
+ c_args.dest_offset = args->offset + nbytes;
+ c_args.continuity_counter = args->continuity_counter;
+ c_args.crc = cpu_to_be32(crc);
+
+ /* Write the CRC32 at the end */
+ nbytes += table_section_crc32_write_into(&c_args);
+
+ return nbytes;
+}
+
+void
+vidtv_psi_pat_table_destroy(struct vidtv_psi_table_pat *p)
+{
+ vidtv_psi_pat_program_destroy(p->program);
+ kfree(p);
+}
+
+struct vidtv_psi_table_pmt_stream*
+vidtv_psi_pmt_stream_init(struct vidtv_psi_table_pmt_stream *head,
+ enum vidtv_psi_stream_types stream_type,
+ u16 es_pid)
+{
+ struct vidtv_psi_table_pmt_stream *stream;
+ const u16 RESERVED1 = 0x07;
+ const u16 RESERVED2 = 0x0f;
+ const u16 ZERO = 0x0;
+ u16 desc_loop_len;
+
+ stream = kzalloc(sizeof(*stream), GFP_KERNEL);
+ if (!stream)
+ return NULL;
+
+ stream->type = stream_type;
+
+ stream->bitfield = cpu_to_be16((RESERVED1 << 13) | es_pid);
+
+ desc_loop_len = vidtv_psi_desc_comp_loop_len(stream->descriptor);
+
+ stream->bitfield2 = cpu_to_be16((RESERVED2 << 12) |
+ (ZERO << 10) |
+ desc_loop_len);
+ stream->next = NULL;
+
+ if (head) {
+ while (head->next)
+ head = head->next;
+
+ head->next = stream;
+ }
+
+ return stream;
+}
+
+void vidtv_psi_pmt_stream_destroy(struct vidtv_psi_table_pmt_stream *s)
+{
+ struct vidtv_psi_table_pmt_stream *tmp_stream = NULL;
+ struct vidtv_psi_table_pmt_stream *curr_stream = s;
+
+ while (curr_stream) {
+ tmp_stream = curr_stream;
+ curr_stream = curr_stream->next;
+ vidtv_psi_desc_destroy(tmp_stream->descriptor);
+ kfree(tmp_stream);
+ }
+}
+
+void vidtv_psi_pmt_stream_assign(struct vidtv_psi_table_pmt *pmt,
+ struct vidtv_psi_table_pmt_stream *s)
+{
+ do {
+ /* This function transfers ownership of s to the table */
+ if (s == pmt->stream)
+ return;
+
+ pmt->stream = s;
+ vidtv_psi_pmt_table_update_sec_len(pmt);
+
+ s = NULL;
+ } while (vidtv_psi_get_sec_len(&pmt->header) > MAX_SECTION_LEN);
+
+ vidtv_psi_update_version_num(&pmt->header);
+}
+
+u16 vidtv_psi_pmt_get_pid(struct vidtv_psi_table_pmt *section,
+ struct vidtv_psi_table_pat *pat)
+{
+ struct vidtv_psi_table_pat_program *program = pat->program;
+
+ /*
+ * service_id is the same as program_number in the
+ * corresponding program_map_section
+ * see ETSI EN 300 468 v1.15.1 p. 24
+ */
+ while (program) {
+ if (program->service_id == section->header.id)
+ return vidtv_psi_get_pat_program_pid(program);
+
+ program = program->next;
+ }
+
+ return TS_LAST_VALID_PID + 1; /* not found */
+}
+
+struct vidtv_psi_table_pmt *vidtv_psi_pmt_table_init(u16 program_number,
+ u16 pcr_pid)
+{
+ struct vidtv_psi_table_pmt *pmt;
+ const u16 RESERVED1 = 0x07;
+ const u16 RESERVED2 = 0x0f;
+ const u16 SYNTAX = 0x1;
+ const u16 ONES = 0x03;
+ const u16 ZERO = 0x0;
+ u16 desc_loop_len;
+
+ pmt = kzalloc(sizeof(*pmt), GFP_KERNEL);
+ if (!pmt)
+ return NULL;
+
+ if (!pcr_pid)
+ pcr_pid = 0x1fff;
+
+ pmt->header.table_id = 0x2;
+
+ pmt->header.bitfield = cpu_to_be16((SYNTAX << 15) | (ZERO << 14) | (ONES << 12));
+
+ pmt->header.id = cpu_to_be16(program_number);
+ pmt->header.current_next = 0x1;
+
+ pmt->header.version = 0x1f;
+
+ pmt->header.one2 = ONES;
+ pmt->header.section_id = 0;
+ pmt->header.last_section = 0;
+
+ pmt->bitfield = cpu_to_be16((RESERVED1 << 13) | pcr_pid);
+
+ desc_loop_len = vidtv_psi_desc_comp_loop_len(pmt->descriptor);
+
+ pmt->bitfield2 = cpu_to_be16((RESERVED2 << 12) |
+ (ZERO << 10) |
+ desc_loop_len);
+
+ vidtv_psi_pmt_table_update_sec_len(pmt);
+
+ return pmt;
+}
+
+u32 vidtv_psi_pmt_write_into(struct vidtv_psi_pmt_write_args *args)
+{
+ struct vidtv_psi_desc *table_descriptor = args->pmt->descriptor;
+ struct vidtv_psi_table_pmt_stream *stream = args->pmt->stream;
+ struct vidtv_psi_desc *stream_descriptor;
+ u32 crc = INITIAL_CRC;
+ u32 nbytes = 0;
+ struct header_write_args h_args = {
+ .dest_buf = args->buf,
+ .dest_offset = args->offset,
+ .h = &args->pmt->header,
+ .pid = args->pid,
+ .continuity_counter = args->continuity_counter,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct psi_write_args psi_args = {
+ .dest_buf = args->buf,
+ .from = &args->pmt->bitfield,
+ .len = sizeof_field(struct vidtv_psi_table_pmt, bitfield) +
+ sizeof_field(struct vidtv_psi_table_pmt, bitfield2),
+ .pid = args->pid,
+ .new_psi_section = false,
+ .is_crc = false,
+ .dest_buf_sz = args->buf_sz,
+ .crc = &crc,
+ };
+ struct desc_write_args d_args = {
+ .dest_buf = args->buf,
+ .desc = table_descriptor,
+ .pid = args->pid,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct crc32_write_args c_args = {
+ .dest_buf = args->buf,
+ .pid = args->pid,
+ .dest_buf_sz = args->buf_sz,
+ };
+
+ vidtv_psi_pmt_table_update_sec_len(args->pmt);
+
+ h_args.crc = &crc;
+
+ nbytes += vidtv_psi_table_header_write_into(&h_args);
+
+ /* write the two bitfields */
+ psi_args.dest_offset = args->offset + nbytes;
+ psi_args.continuity_counter = args->continuity_counter;
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ while (table_descriptor) {
+ /* write the descriptors, if any */
+ d_args.dest_offset = args->offset + nbytes;
+ d_args.continuity_counter = args->continuity_counter;
+ d_args.crc = &crc;
+
+ nbytes += vidtv_psi_desc_write_into(&d_args);
+
+ table_descriptor = table_descriptor->next;
+ }
+
+ psi_args.len += sizeof_field(struct vidtv_psi_table_pmt_stream, type);
+ while (stream) {
+ /* write the streams, if any */
+ psi_args.from = stream;
+ psi_args.dest_offset = args->offset + nbytes;
+ psi_args.continuity_counter = args->continuity_counter;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ stream_descriptor = stream->descriptor;
+
+ while (stream_descriptor) {
+ /* write the stream descriptors, if any */
+ d_args.dest_offset = args->offset + nbytes;
+ d_args.desc = stream_descriptor;
+ d_args.continuity_counter = args->continuity_counter;
+ d_args.crc = &crc;
+
+ nbytes += vidtv_psi_desc_write_into(&d_args);
+
+ stream_descriptor = stream_descriptor->next;
+ }
+
+ stream = stream->next;
+ }
+
+ c_args.dest_offset = args->offset + nbytes;
+ c_args.crc = cpu_to_be32(crc);
+ c_args.continuity_counter = args->continuity_counter;
+
+ /* Write the CRC32 at the end */
+ nbytes += table_section_crc32_write_into(&c_args);
+
+ return nbytes;
+}
+
+void vidtv_psi_pmt_table_destroy(struct vidtv_psi_table_pmt *pmt)
+{
+ vidtv_psi_desc_destroy(pmt->descriptor);
+ vidtv_psi_pmt_stream_destroy(pmt->stream);
+ kfree(pmt);
+}
+
+struct vidtv_psi_table_sdt *vidtv_psi_sdt_table_init(u16 network_id,
+ u16 transport_stream_id)
+{
+ struct vidtv_psi_table_sdt *sdt;
+ const u16 RESERVED = 0xff;
+ const u16 SYNTAX = 0x1;
+ const u16 ONES = 0x03;
+ const u16 ONE = 0x1;
+
+ sdt = kzalloc(sizeof(*sdt), GFP_KERNEL);
+ if (!sdt)
+ return NULL;
+
+ sdt->header.table_id = 0x42;
+ sdt->header.bitfield = cpu_to_be16((SYNTAX << 15) | (ONE << 14) | (ONES << 12));
+
+ /*
+ * This is a 16-bit field which serves as a label for identification
+ * of the TS, about which the SDT informs, from any other multiplex
+ * within the delivery system.
+ */
+ sdt->header.id = cpu_to_be16(transport_stream_id);
+ sdt->header.current_next = ONE;
+
+ sdt->header.version = 0x1f;
+
+ sdt->header.one2 = ONES;
+ sdt->header.section_id = 0;
+ sdt->header.last_section = 0;
+
+ /*
+ * FIXME: The network_id range from 0xFF01 to 0xFFFF is used to
+ * indicate temporary private use. For now, let's use the first
+ * value.
+ * This can be changed to something more useful, when support for
+ * NIT gets added
+ */
+ sdt->network_id = cpu_to_be16(network_id);
+ sdt->reserved = RESERVED;
+
+ vidtv_psi_sdt_table_update_sec_len(sdt);
+
+ return sdt;
+}
+
+u32 vidtv_psi_sdt_write_into(struct vidtv_psi_sdt_write_args *args)
+{
+ struct header_write_args h_args = {
+ .dest_buf = args->buf,
+ .dest_offset = args->offset,
+ .h = &args->sdt->header,
+ .pid = VIDTV_SDT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct psi_write_args psi_args = {
+ .dest_buf = args->buf,
+ .len = sizeof_field(struct vidtv_psi_table_sdt, network_id) +
+ sizeof_field(struct vidtv_psi_table_sdt, reserved),
+ .pid = VIDTV_SDT_PID,
+ .new_psi_section = false,
+ .is_crc = false,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct desc_write_args d_args = {
+ .dest_buf = args->buf,
+ .pid = VIDTV_SDT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct crc32_write_args c_args = {
+ .dest_buf = args->buf,
+ .pid = VIDTV_SDT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct vidtv_psi_table_sdt_service *service = args->sdt->service;
+ struct vidtv_psi_desc *service_desc;
+ u32 nbytes = 0;
+ u32 crc = INITIAL_CRC;
+
+ /* see ETSI EN 300 468 v1.15.1 p. 11 */
+
+ vidtv_psi_sdt_table_update_sec_len(args->sdt);
+
+ h_args.continuity_counter = args->continuity_counter;
+ h_args.crc = &crc;
+
+ nbytes += vidtv_psi_table_header_write_into(&h_args);
+
+ psi_args.from = &args->sdt->network_id;
+ psi_args.dest_offset = args->offset + nbytes;
+ psi_args.continuity_counter = args->continuity_counter;
+ psi_args.crc = &crc;
+
+ /* copy u16 network_id + u8 reserved)*/
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ /* skip both pointers at the end */
+ psi_args.len = sizeof(struct vidtv_psi_table_sdt_service) -
+ sizeof(struct vidtv_psi_desc *) -
+ sizeof(struct vidtv_psi_table_sdt_service *);
+
+ while (service) {
+ /* copy the services, if any */
+ psi_args.from = service;
+ psi_args.dest_offset = args->offset + nbytes;
+ psi_args.continuity_counter = args->continuity_counter;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ service_desc = service->descriptor;
+
+ while (service_desc) {
+ /* copy the service descriptors, if any */
+ d_args.dest_offset = args->offset + nbytes;
+ d_args.desc = service_desc;
+ d_args.continuity_counter = args->continuity_counter;
+ d_args.crc = &crc;
+
+ nbytes += vidtv_psi_desc_write_into(&d_args);
+
+ service_desc = service_desc->next;
+ }
+
+ service = service->next;
+ }
+
+ c_args.dest_offset = args->offset + nbytes;
+ c_args.crc = cpu_to_be32(crc);
+ c_args.continuity_counter = args->continuity_counter;
+
+ /* Write the CRC at the end */
+ nbytes += table_section_crc32_write_into(&c_args);
+
+ return nbytes;
+}
+
+void vidtv_psi_sdt_table_destroy(struct vidtv_psi_table_sdt *sdt)
+{
+ vidtv_psi_sdt_service_destroy(sdt->service);
+ kfree(sdt);
+}
+
+struct vidtv_psi_table_sdt_service
+*vidtv_psi_sdt_service_init(struct vidtv_psi_table_sdt_service *head,
+ u16 service_id,
+ bool eit_schedule,
+ bool eit_present_following)
+{
+ struct vidtv_psi_table_sdt_service *service;
+
+ service = kzalloc(sizeof(*service), GFP_KERNEL);
+ if (!service)
+ return NULL;
+
+ /*
+ * ETSI 300 468: this is a 16bit field which serves as a label to
+ * identify this service from any other service within the TS.
+ * The service id is the same as the program number in the
+ * corresponding program_map_section
+ */
+ service->service_id = cpu_to_be16(service_id);
+ service->EIT_schedule = eit_schedule;
+ service->EIT_present_following = eit_present_following;
+ service->reserved = 0x3f;
+
+ service->bitfield = cpu_to_be16(RUNNING << 13);
+
+ if (head) {
+ while (head->next)
+ head = head->next;
+
+ head->next = service;
+ }
+
+ return service;
+}
+
+void
+vidtv_psi_sdt_service_destroy(struct vidtv_psi_table_sdt_service *service)
+{
+ struct vidtv_psi_table_sdt_service *curr = service;
+ struct vidtv_psi_table_sdt_service *tmp = NULL;
+
+ while (curr) {
+ tmp = curr;
+ curr = curr->next;
+ vidtv_psi_desc_destroy(tmp->descriptor);
+ kfree(tmp);
+ }
+}
+
+void
+vidtv_psi_sdt_service_assign(struct vidtv_psi_table_sdt *sdt,
+ struct vidtv_psi_table_sdt_service *service)
+{
+ do {
+ if (service == sdt->service)
+ return;
+
+ sdt->service = service;
+
+ /* recompute section length */
+ vidtv_psi_sdt_table_update_sec_len(sdt);
+
+ service = NULL;
+ } while (vidtv_psi_get_sec_len(&sdt->header) > MAX_SECTION_LEN);
+
+ vidtv_psi_update_version_num(&sdt->header);
+}
+
+/*
+ * PMTs contain information about programs. For each program,
+ * there is one PMT section. This function will create a section
+ * for each program found in the PAT
+ */
+struct vidtv_psi_table_pmt**
+vidtv_psi_pmt_create_sec_for_each_pat_entry(struct vidtv_psi_table_pat *pat,
+ u16 pcr_pid)
+
+{
+ struct vidtv_psi_table_pat_program *program;
+ struct vidtv_psi_table_pmt **pmt_secs;
+ u32 i = 0, num_pmt = 0;
+
+ /*
+ * The number of PMT entries is the number of PAT entries
+ * that contain service_id. That exclude special tables, like NIT
+ */
+ program = pat->program;
+ while (program) {
+ if (program->service_id)
+ num_pmt++;
+ program = program->next;
+ }
+
+ pmt_secs = kcalloc(num_pmt,
+ sizeof(struct vidtv_psi_table_pmt *),
+ GFP_KERNEL);
+ if (!pmt_secs)
+ return NULL;
+
+ for (program = pat->program; program; program = program->next) {
+ if (!program->service_id)
+ continue;
+ pmt_secs[i] = vidtv_psi_pmt_table_init(be16_to_cpu(program->service_id),
+ pcr_pid);
+
+ if (!pmt_secs[i]) {
+ while (i > 0) {
+ i--;
+ vidtv_psi_pmt_table_destroy(pmt_secs[i]);
+ }
+ return NULL;
+ }
+ i++;
+ }
+ pat->num_pmt = num_pmt;
+
+ return pmt_secs;
+}
+
+/* find the PMT section associated with 'program_num' */
+struct vidtv_psi_table_pmt
+*vidtv_psi_find_pmt_sec(struct vidtv_psi_table_pmt **pmt_sections,
+ u16 nsections,
+ u16 program_num)
+{
+ struct vidtv_psi_table_pmt *sec = NULL;
+ u32 i;
+
+ for (i = 0; i < nsections; ++i) {
+ sec = pmt_sections[i];
+ if (be16_to_cpu(sec->header.id) == program_num)
+ return sec;
+ }
+
+ return NULL; /* not found */
+}
+
+static void vidtv_psi_nit_table_update_sec_len(struct vidtv_psi_table_nit *nit)
+{
+ u16 length = 0;
+ struct vidtv_psi_table_transport *t = nit->transport;
+ u16 desc_loop_len;
+ u16 transport_loop_len = 0;
+
+ /*
+ * from immediately after 'section_length' until
+ * 'network_descriptor_length'
+ */
+ length += NIT_LEN_UNTIL_NETWORK_DESCRIPTOR_LEN;
+
+ desc_loop_len = vidtv_psi_desc_comp_loop_len(nit->descriptor);
+ vidtv_psi_set_desc_loop_len(&nit->bitfield, desc_loop_len, 12);
+
+ length += desc_loop_len;
+
+ length += sizeof_field(struct vidtv_psi_table_nit, bitfield2);
+
+ while (t) {
+ /* skip both pointers at the end */
+ transport_loop_len += sizeof(struct vidtv_psi_table_transport) -
+ sizeof(struct vidtv_psi_desc *) -
+ sizeof(struct vidtv_psi_table_transport *);
+
+ length += transport_loop_len;
+
+ desc_loop_len = vidtv_psi_desc_comp_loop_len(t->descriptor);
+ vidtv_psi_set_desc_loop_len(&t->bitfield, desc_loop_len, 12);
+
+ length += desc_loop_len;
+
+ t = t->next;
+ }
+
+ // Actually sets the transport stream loop len, maybe rename this function later
+ vidtv_psi_set_desc_loop_len(&nit->bitfield2, transport_loop_len, 12);
+ length += CRC_SIZE_IN_BYTES;
+
+ vidtv_psi_set_sec_len(&nit->header, length);
+}
+
+struct vidtv_psi_table_nit
+*vidtv_psi_nit_table_init(u16 network_id,
+ u16 transport_stream_id,
+ char *network_name,
+ struct vidtv_psi_desc_service_list_entry *service_list)
+{
+ struct vidtv_psi_table_transport *transport;
+ struct vidtv_psi_table_nit *nit;
+ const u16 SYNTAX = 0x1;
+ const u16 ONES = 0x03;
+ const u16 ONE = 0x1;
+
+ nit = kzalloc(sizeof(*nit), GFP_KERNEL);
+ if (!nit)
+ return NULL;
+
+ transport = kzalloc(sizeof(*transport), GFP_KERNEL);
+ if (!transport)
+ goto free_nit;
+
+ nit->header.table_id = 0x40; // ACTUAL_NETWORK
+
+ nit->header.bitfield = cpu_to_be16((SYNTAX << 15) | (ONE << 14) | (ONES << 12));
+
+ nit->header.id = cpu_to_be16(network_id);
+ nit->header.current_next = ONE;
+
+ nit->header.version = 0x1f;
+
+ nit->header.one2 = ONES;
+ nit->header.section_id = 0;
+ nit->header.last_section = 0;
+
+ nit->bitfield = cpu_to_be16(0xf);
+ nit->bitfield2 = cpu_to_be16(0xf);
+
+ nit->descriptor = (struct vidtv_psi_desc *)
+ vidtv_psi_network_name_desc_init(NULL, network_name);
+ if (!nit->descriptor)
+ goto free_transport;
+
+ transport->transport_id = cpu_to_be16(transport_stream_id);
+ transport->network_id = cpu_to_be16(network_id);
+ transport->bitfield = cpu_to_be16(0xf);
+ transport->descriptor = (struct vidtv_psi_desc *)
+ vidtv_psi_service_list_desc_init(NULL, service_list);
+ if (!transport->descriptor)
+ goto free_nit_desc;
+
+ nit->transport = transport;
+
+ vidtv_psi_nit_table_update_sec_len(nit);
+
+ return nit;
+
+free_nit_desc:
+ vidtv_psi_desc_destroy((struct vidtv_psi_desc *)nit->descriptor);
+
+free_transport:
+ kfree(transport);
+free_nit:
+ kfree(nit);
+ return NULL;
+}
+
+u32 vidtv_psi_nit_write_into(struct vidtv_psi_nit_write_args *args)
+{
+ struct header_write_args h_args = {
+ .dest_buf = args->buf,
+ .dest_offset = args->offset,
+ .h = &args->nit->header,
+ .pid = VIDTV_NIT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct psi_write_args psi_args = {
+ .dest_buf = args->buf,
+ .from = &args->nit->bitfield,
+ .len = sizeof_field(struct vidtv_psi_table_nit, bitfield),
+ .pid = VIDTV_NIT_PID,
+ .new_psi_section = false,
+ .is_crc = false,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct desc_write_args d_args = {
+ .dest_buf = args->buf,
+ .pid = VIDTV_NIT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct crc32_write_args c_args = {
+ .dest_buf = args->buf,
+ .pid = VIDTV_NIT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct vidtv_psi_desc *table_descriptor = args->nit->descriptor;
+ struct vidtv_psi_table_transport *transport = args->nit->transport;
+ struct vidtv_psi_desc *transport_descriptor;
+ u32 crc = INITIAL_CRC;
+ u32 nbytes = 0;
+
+ vidtv_psi_nit_table_update_sec_len(args->nit);
+
+ h_args.continuity_counter = args->continuity_counter;
+ h_args.crc = &crc;
+
+ nbytes += vidtv_psi_table_header_write_into(&h_args);
+
+ /* write the bitfield */
+
+ psi_args.dest_offset = args->offset + nbytes;
+ psi_args.continuity_counter = args->continuity_counter;
+ psi_args.crc = &crc;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ while (table_descriptor) {
+ /* write the descriptors, if any */
+ d_args.dest_offset = args->offset + nbytes;
+ d_args.desc = table_descriptor;
+ d_args.continuity_counter = args->continuity_counter;
+ d_args.crc = &crc;
+
+ nbytes += vidtv_psi_desc_write_into(&d_args);
+
+ table_descriptor = table_descriptor->next;
+ }
+
+ /* write the second bitfield */
+ psi_args.from = &args->nit->bitfield2;
+ psi_args.len = sizeof_field(struct vidtv_psi_table_nit, bitfield2);
+ psi_args.dest_offset = args->offset + nbytes;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ psi_args.len = sizeof_field(struct vidtv_psi_table_transport, transport_id) +
+ sizeof_field(struct vidtv_psi_table_transport, network_id) +
+ sizeof_field(struct vidtv_psi_table_transport, bitfield);
+ while (transport) {
+ /* write the transport sections, if any */
+ psi_args.from = transport;
+ psi_args.dest_offset = args->offset + nbytes;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ transport_descriptor = transport->descriptor;
+
+ while (transport_descriptor) {
+ /* write the transport descriptors, if any */
+ d_args.dest_offset = args->offset + nbytes;
+ d_args.desc = transport_descriptor;
+ d_args.continuity_counter = args->continuity_counter;
+ d_args.crc = &crc;
+
+ nbytes += vidtv_psi_desc_write_into(&d_args);
+
+ transport_descriptor = transport_descriptor->next;
+ }
+
+ transport = transport->next;
+ }
+
+ c_args.dest_offset = args->offset + nbytes;
+ c_args.crc = cpu_to_be32(crc);
+ c_args.continuity_counter = args->continuity_counter;
+
+ /* Write the CRC32 at the end */
+ nbytes += table_section_crc32_write_into(&c_args);
+
+ return nbytes;
+}
+
+static void vidtv_psi_transport_destroy(struct vidtv_psi_table_transport *t)
+{
+ struct vidtv_psi_table_transport *tmp_t = NULL;
+ struct vidtv_psi_table_transport *curr_t = t;
+
+ while (curr_t) {
+ tmp_t = curr_t;
+ curr_t = curr_t->next;
+ vidtv_psi_desc_destroy(tmp_t->descriptor);
+ kfree(tmp_t);
+ }
+}
+
+void vidtv_psi_nit_table_destroy(struct vidtv_psi_table_nit *nit)
+{
+ vidtv_psi_desc_destroy(nit->descriptor);
+ vidtv_psi_transport_destroy(nit->transport);
+ kfree(nit);
+}
+
+void vidtv_psi_eit_table_update_sec_len(struct vidtv_psi_table_eit *eit)
+{
+ struct vidtv_psi_table_eit_event *e = eit->event;
+ u16 desc_loop_len;
+ u16 length = 0;
+
+ /*
+ * from immediately after 'section_length' until
+ * 'last_table_id'
+ */
+ length += EIT_LEN_UNTIL_LAST_TABLE_ID;
+
+ while (e) {
+ /* skip both pointers at the end */
+ length += sizeof(struct vidtv_psi_table_eit_event) -
+ sizeof(struct vidtv_psi_desc *) -
+ sizeof(struct vidtv_psi_table_eit_event *);
+
+ desc_loop_len = vidtv_psi_desc_comp_loop_len(e->descriptor);
+ vidtv_psi_set_desc_loop_len(&e->bitfield, desc_loop_len, 12);
+
+ length += desc_loop_len;
+
+ e = e->next;
+ }
+
+ length += CRC_SIZE_IN_BYTES;
+
+ vidtv_psi_set_sec_len(&eit->header, length);
+}
+
+void vidtv_psi_eit_event_assign(struct vidtv_psi_table_eit *eit,
+ struct vidtv_psi_table_eit_event *e)
+{
+ do {
+ if (e == eit->event)
+ return;
+
+ eit->event = e;
+ vidtv_psi_eit_table_update_sec_len(eit);
+
+ e = NULL;
+ } while (vidtv_psi_get_sec_len(&eit->header) > EIT_MAX_SECTION_LEN);
+
+ vidtv_psi_update_version_num(&eit->header);
+}
+
+struct vidtv_psi_table_eit
+*vidtv_psi_eit_table_init(u16 network_id,
+ u16 transport_stream_id,
+ __be16 service_id)
+{
+ struct vidtv_psi_table_eit *eit;
+ const u16 SYNTAX = 0x1;
+ const u16 ONE = 0x1;
+ const u16 ONES = 0x03;
+
+ eit = kzalloc(sizeof(*eit), GFP_KERNEL);
+ if (!eit)
+ return NULL;
+
+ eit->header.table_id = 0x4e; //actual_transport_stream: present/following
+
+ eit->header.bitfield = cpu_to_be16((SYNTAX << 15) | (ONE << 14) | (ONES << 12));
+
+ eit->header.id = service_id;
+ eit->header.current_next = ONE;
+
+ eit->header.version = 0x1f;
+
+ eit->header.one2 = ONES;
+ eit->header.section_id = 0;
+ eit->header.last_section = 0;
+
+ eit->transport_id = cpu_to_be16(transport_stream_id);
+ eit->network_id = cpu_to_be16(network_id);
+
+ eit->last_segment = eit->header.last_section; /* not implemented */
+ eit->last_table_id = eit->header.table_id; /* not implemented */
+
+ vidtv_psi_eit_table_update_sec_len(eit);
+
+ return eit;
+}
+
+u32 vidtv_psi_eit_write_into(struct vidtv_psi_eit_write_args *args)
+{
+ struct header_write_args h_args = {
+ .dest_buf = args->buf,
+ .dest_offset = args->offset,
+ .h = &args->eit->header,
+ .pid = VIDTV_EIT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct psi_write_args psi_args = {
+ .dest_buf = args->buf,
+ .len = sizeof_field(struct vidtv_psi_table_eit, transport_id) +
+ sizeof_field(struct vidtv_psi_table_eit, network_id) +
+ sizeof_field(struct vidtv_psi_table_eit, last_segment) +
+ sizeof_field(struct vidtv_psi_table_eit, last_table_id),
+ .pid = VIDTV_EIT_PID,
+ .new_psi_section = false,
+ .is_crc = false,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct desc_write_args d_args = {
+ .dest_buf = args->buf,
+ .pid = VIDTV_EIT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct crc32_write_args c_args = {
+ .dest_buf = args->buf,
+ .pid = VIDTV_EIT_PID,
+ .dest_buf_sz = args->buf_sz,
+ };
+ struct vidtv_psi_table_eit_event *event = args->eit->event;
+ struct vidtv_psi_desc *event_descriptor;
+ u32 crc = INITIAL_CRC;
+ u32 nbytes = 0;
+
+ vidtv_psi_eit_table_update_sec_len(args->eit);
+
+ h_args.continuity_counter = args->continuity_counter;
+ h_args.crc = &crc;
+
+ nbytes += vidtv_psi_table_header_write_into(&h_args);
+
+ psi_args.from = &args->eit->transport_id;
+ psi_args.dest_offset = args->offset + nbytes;
+ psi_args.continuity_counter = args->continuity_counter;
+ psi_args.crc = &crc;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ /* skip both pointers at the end */
+ psi_args.len = sizeof(struct vidtv_psi_table_eit_event) -
+ sizeof(struct vidtv_psi_desc *) -
+ sizeof(struct vidtv_psi_table_eit_event *);
+ while (event) {
+ /* copy the events, if any */
+ psi_args.from = event;
+ psi_args.dest_offset = args->offset + nbytes;
+
+ nbytes += vidtv_psi_ts_psi_write_into(&psi_args);
+
+ event_descriptor = event->descriptor;
+
+ while (event_descriptor) {
+ /* copy the event descriptors, if any */
+ d_args.dest_offset = args->offset + nbytes;
+ d_args.desc = event_descriptor;
+ d_args.continuity_counter = args->continuity_counter;
+ d_args.crc = &crc;
+
+ nbytes += vidtv_psi_desc_write_into(&d_args);
+
+ event_descriptor = event_descriptor->next;
+ }
+
+ event = event->next;
+ }
+
+ c_args.dest_offset = args->offset + nbytes;
+ c_args.crc = cpu_to_be32(crc);
+ c_args.continuity_counter = args->continuity_counter;
+
+ /* Write the CRC at the end */
+ nbytes += table_section_crc32_write_into(&c_args);
+
+ return nbytes;
+}
+
+struct vidtv_psi_table_eit_event
+*vidtv_psi_eit_event_init(struct vidtv_psi_table_eit_event *head, u16 event_id)
+{
+ const u8 DURATION[] = {0x23, 0x59, 0x59}; /* BCD encoded */
+ struct vidtv_psi_table_eit_event *e;
+ struct timespec64 ts;
+ struct tm time;
+ int mjd, l;
+ __be16 mjd_be;
+
+ e = kzalloc(sizeof(*e), GFP_KERNEL);
+ if (!e)
+ return NULL;
+
+ e->event_id = cpu_to_be16(event_id);
+
+ ts = ktime_to_timespec64(ktime_get_real());
+ time64_to_tm(ts.tv_sec, 0, &time);
+
+ /* Convert date to Modified Julian Date - per EN 300 468 Annex C */
+ if (time.tm_mon < 2)
+ l = 1;
+ else
+ l = 0;
+
+ mjd = 14956 + time.tm_mday;
+ mjd += (time.tm_year - l) * 36525 / 100;
+ mjd += (time.tm_mon + 2 + l * 12) * 306001 / 10000;
+ mjd_be = cpu_to_be16(mjd);
+
+ /*
+ * Store MJD and hour/min/sec to the event.
+ *
+ * Let's make the event to start on a full hour
+ */
+ memcpy(e->start_time, &mjd_be, sizeof(mjd_be));
+ e->start_time[2] = bin2bcd(time.tm_hour);
+ e->start_time[3] = 0;
+ e->start_time[4] = 0;
+
+ /*
+ * TODO: for now, the event will last for a day. Should be
+ * enough for testing purposes, but if one runs the driver
+ * for more than that, the current event will become invalid.
+ * So, we need a better code here in order to change the start
+ * time once the event expires.
+ */
+ memcpy(e->duration, DURATION, sizeof(e->duration));
+
+ e->bitfield = cpu_to_be16(RUNNING << 13);
+
+ if (head) {
+ while (head->next)
+ head = head->next;
+
+ head->next = e;
+ }
+
+ return e;
+}
+
+void vidtv_psi_eit_event_destroy(struct vidtv_psi_table_eit_event *e)
+{
+ struct vidtv_psi_table_eit_event *tmp_e = NULL;
+ struct vidtv_psi_table_eit_event *curr_e = e;
+
+ while (curr_e) {
+ tmp_e = curr_e;
+ curr_e = curr_e->next;
+ vidtv_psi_desc_destroy(tmp_e->descriptor);
+ kfree(tmp_e);
+ }
+}
+
+void vidtv_psi_eit_table_destroy(struct vidtv_psi_table_eit *eit)
+{
+ vidtv_psi_eit_event_destroy(eit->event);
+ kfree(eit);
+}
diff --git a/drivers/media/test-drivers/vidtv/vidtv_psi.h b/drivers/media/test-drivers/vidtv/vidtv_psi.h
new file mode 100644
index 0000000..fdc825e
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_psi.h
@@ -0,0 +1,809 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This file contains the logic to work with MPEG Program-Specific Information.
+ * These are defined both in ISO/IEC 13818-1 (systems) and ETSI EN 300 468.
+ * PSI is carried in the form of table structures, and although each table might
+ * technically be broken into one or more sections, we do not do this here,
+ * hence 'table' and 'section' are interchangeable for vidtv.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#ifndef VIDTV_PSI_H
+#define VIDTV_PSI_H
+
+#include <linux/types.h>
+
+/*
+ * all section lengths start immediately after the 'section_length' field
+ * see ISO/IEC 13818-1 : 2000 and ETSI EN 300 468 V 1.10.1 for
+ * reference
+ */
+#define PAT_LEN_UNTIL_LAST_SECTION_NUMBER 5
+#define PMT_LEN_UNTIL_PROGRAM_INFO_LENGTH 9
+#define SDT_LEN_UNTIL_RESERVED_FOR_FUTURE_USE 8
+#define NIT_LEN_UNTIL_NETWORK_DESCRIPTOR_LEN 7
+#define EIT_LEN_UNTIL_LAST_TABLE_ID 11
+#define MAX_SECTION_LEN 1021
+#define EIT_MAX_SECTION_LEN 4093 /* see ETSI 300 468 v.1.10.1 p. 26 */
+#define VIDTV_PAT_PID 0 /* mandated by the specs */
+#define VIDTV_SDT_PID 0x0011 /* mandated by the specs */
+#define VIDTV_NIT_PID 0x0010 /* mandated by the specs */
+#define VIDTV_EIT_PID 0x0012 /*mandated by the specs */
+
+enum vidtv_psi_descriptors {
+ REGISTRATION_DESCRIPTOR = 0x05, /* See ISO/IEC 13818-1 section 2.6.8 */
+ NETWORK_NAME_DESCRIPTOR = 0x40, /* See ETSI EN 300 468 section 6.2.27 */
+ SERVICE_LIST_DESCRIPTOR = 0x41, /* See ETSI EN 300 468 section 6.2.35 */
+ SERVICE_DESCRIPTOR = 0x48, /* See ETSI EN 300 468 section 6.2.33 */
+ SHORT_EVENT_DESCRIPTOR = 0x4d, /* See ETSI EN 300 468 section 6.2.37 */
+};
+
+enum vidtv_psi_stream_types {
+ STREAM_PRIVATE_DATA = 0x06, /* see ISO/IEC 13818-1 2000 p. 48 */
+};
+
+/*
+ * struct vidtv_psi_desc - A generic PSI descriptor type.
+ * The descriptor length is an 8-bit field specifying the total number of bytes of the data portion
+ * of the descriptor following the byte defining the value of this field.
+ */
+struct vidtv_psi_desc {
+ struct vidtv_psi_desc *next;
+ u8 type;
+ u8 length;
+ u8 data[];
+} __packed;
+
+/*
+ * struct vidtv_psi_desc_service - Service descriptor.
+ * See ETSI EN 300 468 section 6.2.33.
+ */
+struct vidtv_psi_desc_service {
+ struct vidtv_psi_desc *next;
+ u8 type;
+ u8 length;
+
+ u8 service_type;
+ u8 provider_name_len;
+ char *provider_name;
+ u8 service_name_len;
+ char *service_name;
+} __packed;
+
+/*
+ * struct vidtv_psi_desc_registration - A registration descriptor.
+ * See ISO/IEC 13818-1 section 2.6.8
+ */
+struct vidtv_psi_desc_registration {
+ struct vidtv_psi_desc *next;
+ u8 type;
+ u8 length;
+
+ /*
+ * The format_identifier is a 32-bit value obtained from a Registration
+ * Authority as designated by ISO/IEC JTC 1/SC 29.
+ */
+ __be32 format_id;
+ /*
+ * The meaning of additional_identification_info bytes, if any, are
+ * defined by the assignee of that format_identifier, and once defined
+ * they shall not change.
+ */
+ u8 additional_identification_info[];
+} __packed;
+
+/*
+ * struct vidtv_psi_desc_network_name - A network name descriptor
+ * see ETSI EN 300 468 v1.15.1 section 6.2.27
+ */
+struct vidtv_psi_desc_network_name {
+ struct vidtv_psi_desc *next;
+ u8 type;
+ u8 length;
+ char *network_name;
+} __packed;
+
+struct vidtv_psi_desc_service_list_entry {
+ __be16 service_id;
+ u8 service_type;
+ struct vidtv_psi_desc_service_list_entry *next;
+} __packed;
+
+/*
+ * struct vidtv_psi_desc_service_list - A service list descriptor
+ * see ETSI EN 300 468 v1.15.1 section 6.2.35
+ */
+struct vidtv_psi_desc_service_list {
+ struct vidtv_psi_desc *next;
+ u8 type;
+ u8 length;
+ struct vidtv_psi_desc_service_list_entry *service_list;
+} __packed;
+
+/*
+ * struct vidtv_psi_desc_short_event - A short event descriptor
+ * see ETSI EN 300 468 v1.15.1 section 6.2.37
+ */
+struct vidtv_psi_desc_short_event {
+ struct vidtv_psi_desc *next;
+ u8 type;
+ u8 length;
+ char *iso_language_code;
+ u8 event_name_len;
+ char *event_name;
+ u8 text_len;
+ char *text;
+} __packed;
+
+struct vidtv_psi_desc_short_event
+*vidtv_psi_short_event_desc_init(struct vidtv_psi_desc *head,
+ char *iso_language_code,
+ char *event_name,
+ char *text);
+
+/*
+ * struct vidtv_psi_table_header - A header that is present for all PSI tables.
+ */
+struct vidtv_psi_table_header {
+ u8 table_id;
+
+ __be16 bitfield; /* syntax: 1, zero: 1, one: 2, section_length: 13 */
+
+ __be16 id; /* TS ID */
+ u8 current_next:1;
+ u8 version:5;
+ u8 one2:2;
+ u8 section_id; /* section_number */
+ u8 last_section; /* last_section_number */
+} __packed;
+
+/*
+ * struct vidtv_psi_table_pat_program - A single program in the PAT
+ * See ISO/IEC 13818-1 : 2000 p.43
+ */
+struct vidtv_psi_table_pat_program {
+ __be16 service_id;
+ __be16 bitfield; /* reserved: 3, program_map_pid/network_pid: 13 */
+ struct vidtv_psi_table_pat_program *next;
+} __packed;
+
+/*
+ * struct vidtv_psi_table_pat - The Program Allocation Table (PAT)
+ * See ISO/IEC 13818-1 : 2000 p.43
+ */
+struct vidtv_psi_table_pat {
+ struct vidtv_psi_table_header header;
+ u16 num_pat;
+ u16 num_pmt;
+ struct vidtv_psi_table_pat_program *program;
+} __packed;
+
+/*
+ * struct vidtv_psi_table_sdt_service - Represents a service in the SDT.
+ * see ETSI EN 300 468 v1.15.1 section 5.2.3.
+ */
+struct vidtv_psi_table_sdt_service {
+ __be16 service_id;
+ u8 EIT_present_following:1;
+ u8 EIT_schedule:1;
+ u8 reserved:6;
+ __be16 bitfield; /* running_status: 3, free_ca:1, desc_loop_len:12 */
+ struct vidtv_psi_desc *descriptor;
+ struct vidtv_psi_table_sdt_service *next;
+} __packed;
+
+/*
+ * struct vidtv_psi_table_sdt - Represents the Service Description Table
+ * see ETSI EN 300 468 v1.15.1 section 5.2.3.
+ */
+
+struct vidtv_psi_table_sdt {
+ struct vidtv_psi_table_header header;
+ __be16 network_id; /* original_network_id */
+ u8 reserved;
+ struct vidtv_psi_table_sdt_service *service;
+} __packed;
+
+/*
+ * enum service_running_status - Status of a SDT service.
+ * see ETSI EN 300 468 v1.15.1 section 5.2.3 table 6.
+ */
+enum service_running_status {
+ RUNNING = 0x4,
+};
+
+/*
+ * enum service_type - The type of a SDT service.
+ * see ETSI EN 300 468 v1.15.1 section 6.2.33, table 81.
+ */
+enum service_type {
+ /* see ETSI EN 300 468 v1.15.1 p. 77 */
+ DIGITAL_TELEVISION_SERVICE = 0x1,
+ DIGITAL_RADIO_SOUND_SERVICE = 0X2,
+};
+
+/*
+ * struct vidtv_psi_table_pmt_stream - A single stream in the PMT.
+ * See ISO/IEC 13818-1 : 2000 p.46.
+ */
+struct vidtv_psi_table_pmt_stream {
+ u8 type;
+ __be16 bitfield; /* reserved: 3, elementary_pid: 13 */
+ __be16 bitfield2; /*reserved: 4, zero: 2, desc_length: 10 */
+ struct vidtv_psi_desc *descriptor;
+ struct vidtv_psi_table_pmt_stream *next;
+} __packed;
+
+/*
+ * struct vidtv_psi_table_pmt - The Program Map Table (PMT).
+ * See ISO/IEC 13818-1 : 2000 p.46.
+ */
+struct vidtv_psi_table_pmt {
+ struct vidtv_psi_table_header header;
+ __be16 bitfield; /* reserved:3, pcr_pid: 13 */
+ __be16 bitfield2; /* reserved: 4, zero: 2, desc_len: 10 */
+ struct vidtv_psi_desc *descriptor;
+ struct vidtv_psi_table_pmt_stream *stream;
+} __packed;
+
+/**
+ * struct psi_write_args - Arguments for the PSI packetizer.
+ * @dest_buf: The buffer to write into.
+ * @from: PSI data to be copied.
+ * @len: How much to write.
+ * @dest_offset: where to start writing in the dest_buffer.
+ * @pid: TS packet ID.
+ * @new_psi_section: Set when starting a table section.
+ * @continuity_counter: Incremented on every new packet.
+ * @is_crc: Set when writing the CRC at the end.
+ * @dest_buf_sz: The size of the dest_buffer
+ * @crc: a pointer to store the crc for this chunk
+ */
+struct psi_write_args {
+ void *dest_buf;
+ void *from;
+ size_t len;
+ u32 dest_offset;
+ u16 pid;
+ bool new_psi_section;
+ u8 *continuity_counter;
+ bool is_crc;
+ u32 dest_buf_sz;
+ u32 *crc;
+};
+
+/**
+ * struct desc_write_args - Arguments in order to write a descriptor.
+ * @dest_buf: The buffer to write into.
+ * @dest_offset: where to start writing in the dest_buffer.
+ * @desc: A pointer to the descriptor
+ * @pid: TS packet ID.
+ * @continuity_counter: Incremented on every new packet.
+ * @dest_buf_sz: The size of the dest_buffer
+ * @crc: a pointer to store the crc for this chunk
+ */
+struct desc_write_args {
+ void *dest_buf;
+ u32 dest_offset;
+ struct vidtv_psi_desc *desc;
+ u16 pid;
+ u8 *continuity_counter;
+ u32 dest_buf_sz;
+ u32 *crc;
+};
+
+/**
+ * struct crc32_write_args - Arguments in order to write the CRC at the end of
+ * the PSI tables.
+ * @dest_buf: The buffer to write into.
+ * @dest_offset: where to start writing in the dest_buffer.
+ * @crc: the CRC value to write
+ * @pid: TS packet ID.
+ * @continuity_counter: Incremented on every new packet.
+ * @dest_buf_sz: The size of the dest_buffer
+ */
+struct crc32_write_args {
+ void *dest_buf;
+ u32 dest_offset;
+ __be32 crc;
+ u16 pid;
+ u8 *continuity_counter;
+ u32 dest_buf_sz;
+};
+
+/**
+ * struct header_write_args - Arguments in order to write the common table
+ * header
+ * @dest_buf: The buffer to write into.
+ * @dest_offset: where to start writing in the dest_buffer.
+ * @h: a pointer to the header.
+ * @pid: TS packet ID.
+ * @continuity_counter: Incremented on every new packet.
+ * @dest_buf_sz: The size of the dest_buffer
+ * @crc: a pointer to store the crc for this chunk
+ */
+struct header_write_args {
+ void *dest_buf;
+ u32 dest_offset;
+ struct vidtv_psi_table_header *h;
+ u16 pid;
+ u8 *continuity_counter;
+ u32 dest_buf_sz;
+ u32 *crc;
+};
+
+struct vidtv_psi_desc_service *vidtv_psi_service_desc_init(struct vidtv_psi_desc *head,
+ enum service_type service_type,
+ char *service_name,
+ char *provider_name);
+
+struct vidtv_psi_desc_registration
+*vidtv_psi_registration_desc_init(struct vidtv_psi_desc *head,
+ __be32 format_id,
+ u8 *additional_ident_info,
+ u32 additional_info_len);
+
+struct vidtv_psi_desc_network_name
+*vidtv_psi_network_name_desc_init(struct vidtv_psi_desc *head, char *network_name);
+
+struct vidtv_psi_desc_service_list
+*vidtv_psi_service_list_desc_init(struct vidtv_psi_desc *head,
+ struct vidtv_psi_desc_service_list_entry *entry);
+
+struct vidtv_psi_table_pat_program
+*vidtv_psi_pat_program_init(struct vidtv_psi_table_pat_program *head,
+ u16 service_id,
+ u16 program_map_pid);
+
+struct vidtv_psi_table_pmt_stream*
+vidtv_psi_pmt_stream_init(struct vidtv_psi_table_pmt_stream *head,
+ enum vidtv_psi_stream_types stream_type,
+ u16 es_pid);
+
+struct vidtv_psi_table_pat *vidtv_psi_pat_table_init(u16 transport_stream_id);
+
+struct vidtv_psi_table_pmt *vidtv_psi_pmt_table_init(u16 program_number,
+ u16 pcr_pid);
+
+struct vidtv_psi_table_sdt *vidtv_psi_sdt_table_init(u16 network_id,
+ u16 transport_stream_id);
+
+struct vidtv_psi_table_sdt_service*
+vidtv_psi_sdt_service_init(struct vidtv_psi_table_sdt_service *head,
+ u16 service_id,
+ bool eit_schedule,
+ bool eit_present_following);
+
+void
+vidtv_psi_desc_destroy(struct vidtv_psi_desc *desc);
+
+void
+vidtv_psi_pat_program_destroy(struct vidtv_psi_table_pat_program *p);
+
+void
+vidtv_psi_pat_table_destroy(struct vidtv_psi_table_pat *p);
+
+void
+vidtv_psi_pmt_stream_destroy(struct vidtv_psi_table_pmt_stream *s);
+
+void
+vidtv_psi_pmt_table_destroy(struct vidtv_psi_table_pmt *pmt);
+
+void
+vidtv_psi_sdt_table_destroy(struct vidtv_psi_table_sdt *sdt);
+
+void
+vidtv_psi_sdt_service_destroy(struct vidtv_psi_table_sdt_service *service);
+
+/**
+ * vidtv_psi_sdt_service_assign - Assigns the service loop to the SDT.
+ * @sdt: The SDT to assign to.
+ * @service: The service loop (one or more services)
+ *
+ * This will free the previous service loop in the table.
+ * This will assign ownership of the service loop to the table, i.e. the table
+ * will free this service loop when a call to its destroy function is made.
+ */
+void
+vidtv_psi_sdt_service_assign(struct vidtv_psi_table_sdt *sdt,
+ struct vidtv_psi_table_sdt_service *service);
+
+/**
+ * vidtv_psi_desc_assign - Assigns a descriptor loop at some point
+ * @to: Where to assign this descriptor loop to
+ * @desc: The descriptor loop that will be assigned.
+ *
+ * This will free the loop in 'to', if any.
+ */
+void vidtv_psi_desc_assign(struct vidtv_psi_desc **to,
+ struct vidtv_psi_desc *desc);
+
+/**
+ * vidtv_pmt_desc_assign - Assigns a descriptor loop at some point in a PMT section.
+ * @pmt: The PMT section that will contain the descriptor loop
+ * @to: Where in the PMT to assign this descriptor loop to
+ * @desc: The descriptor loop that will be assigned.
+ *
+ * This will free the loop in 'to', if any.
+ * This will assign ownership of the loop to the table, i.e. the table
+ * will free this loop when a call to its destroy function is made.
+ */
+void vidtv_pmt_desc_assign(struct vidtv_psi_table_pmt *pmt,
+ struct vidtv_psi_desc **to,
+ struct vidtv_psi_desc *desc);
+
+/**
+ * vidtv_sdt_desc_assign - Assigns a descriptor loop at some point in a SDT.
+ * @sdt: The SDT that will contain the descriptor loop
+ * @to: Where in the PMT to assign this descriptor loop to
+ * @desc: The descriptor loop that will be assigned.
+ *
+ * This will free the loop in 'to', if any.
+ * This will assign ownership of the loop to the table, i.e. the table
+ * will free this loop when a call to its destroy function is made.
+ */
+void vidtv_sdt_desc_assign(struct vidtv_psi_table_sdt *sdt,
+ struct vidtv_psi_desc **to,
+ struct vidtv_psi_desc *desc);
+
+/**
+ * vidtv_psi_pat_program_assign - Assigns the program loop to the PAT.
+ * @pat: The PAT to assign to.
+ * @p: The program loop (one or more programs)
+ *
+ * This will free the previous program loop in the table.
+ * This will assign ownership of the program loop to the table, i.e. the table
+ * will free this program loop when a call to its destroy function is made.
+ */
+void vidtv_psi_pat_program_assign(struct vidtv_psi_table_pat *pat,
+ struct vidtv_psi_table_pat_program *p);
+
+/**
+ * vidtv_psi_pmt_stream_assign - Assigns the stream loop to the PAT.
+ * @pmt: The PMT to assign to.
+ * @s: The stream loop (one or more streams)
+ *
+ * This will free the previous stream loop in the table.
+ * This will assign ownership of the stream loop to the table, i.e. the table
+ * will free this stream loop when a call to its destroy function is made.
+ */
+void vidtv_psi_pmt_stream_assign(struct vidtv_psi_table_pmt *pmt,
+ struct vidtv_psi_table_pmt_stream *s);
+
+struct vidtv_psi_desc *vidtv_psi_desc_clone(struct vidtv_psi_desc *desc);
+
+/**
+ * vidtv_psi_pmt_create_sec_for_each_pat_entry - Create a PMT section for each
+ * program found in the PAT
+ * @pat: The PAT to look for programs.
+ * @pcr_pid: packet ID for the PCR to be used for the program described in this
+ * PMT section
+ */
+struct vidtv_psi_table_pmt**
+vidtv_psi_pmt_create_sec_for_each_pat_entry(struct vidtv_psi_table_pat *pat, u16 pcr_pid);
+
+/**
+ * vidtv_psi_pmt_get_pid - Get the TS PID for a PMT section.
+ * @section: The PMT section whose PID we want to retrieve.
+ * @pat: The PAT table to look into.
+ *
+ * Returns: the TS PID for 'section'
+ */
+u16 vidtv_psi_pmt_get_pid(struct vidtv_psi_table_pmt *section,
+ struct vidtv_psi_table_pat *pat);
+
+/**
+ * vidtv_psi_pat_table_update_sec_len - Recompute and update the PAT section length.
+ * @pat: The PAT whose length is to be updated.
+ *
+ * This will traverse the table and accumulate the length of its components,
+ * which is then used to replace the 'section_length' field.
+ *
+ * If section_length > MAX_SECTION_LEN, the operation fails.
+ */
+void vidtv_psi_pat_table_update_sec_len(struct vidtv_psi_table_pat *pat);
+
+/**
+ * vidtv_psi_pmt_table_update_sec_len - Recompute and update the PMT section length.
+ * @pmt: The PMT whose length is to be updated.
+ *
+ * This will traverse the table and accumulate the length of its components,
+ * which is then used to replace the 'section_length' field.
+ *
+ * If section_length > MAX_SECTION_LEN, the operation fails.
+ */
+void vidtv_psi_pmt_table_update_sec_len(struct vidtv_psi_table_pmt *pmt);
+
+/**
+ * vidtv_psi_sdt_table_update_sec_len - Recompute and update the SDT section length.
+ * @sdt: The SDT whose length is to be updated.
+ *
+ * This will traverse the table and accumulate the length of its components,
+ * which is then used to replace the 'section_length' field.
+ *
+ * If section_length > MAX_SECTION_LEN, the operation fails.
+ */
+void vidtv_psi_sdt_table_update_sec_len(struct vidtv_psi_table_sdt *sdt);
+
+/**
+ * struct vidtv_psi_pat_write_args - Arguments for writing a PAT table
+ * @buf: The destination buffer.
+ * @offset: The offset into the destination buffer.
+ * @pat: A pointer to the PAT.
+ * @buf_sz: The size of the destination buffer.
+ * @continuity_counter: A pointer to the CC. Incremented on every new packet.
+ *
+ */
+struct vidtv_psi_pat_write_args {
+ char *buf;
+ u32 offset;
+ struct vidtv_psi_table_pat *pat;
+ u32 buf_sz;
+ u8 *continuity_counter;
+};
+
+/**
+ * vidtv_psi_pat_write_into - Write PAT as MPEG-TS packets into a buffer.
+ * @args: An instance of struct vidtv_psi_pat_write_args
+ *
+ * This function writes the MPEG TS packets for a PAT table into a buffer.
+ * Calling code will usually generate the PAT via a call to its init function
+ * and thus is responsible for freeing it.
+ *
+ * Return: The number of bytes written into the buffer. This is NOT
+ * equal to the size of the PAT, since more space is needed for TS headers during TS
+ * encapsulation.
+ */
+u32 vidtv_psi_pat_write_into(struct vidtv_psi_pat_write_args *args);
+
+/**
+ * struct vidtv_psi_sdt_write_args - Arguments for writing a SDT table
+ * @buf: The destination buffer.
+ * @offset: The offset into the destination buffer.
+ * @sdt: A pointer to the SDT.
+ * @buf_sz: The size of the destination buffer.
+ * @continuity_counter: A pointer to the CC. Incremented on every new packet.
+ *
+ */
+
+struct vidtv_psi_sdt_write_args {
+ char *buf;
+ u32 offset;
+ struct vidtv_psi_table_sdt *sdt;
+ u32 buf_sz;
+ u8 *continuity_counter;
+};
+
+/**
+ * vidtv_psi_sdt_write_into - Write SDT as MPEG-TS packets into a buffer.
+ * @args: an instance of struct vidtv_psi_sdt_write_args
+ *
+ * This function writes the MPEG TS packets for a SDT table into a buffer.
+ * Calling code will usually generate the SDT via a call to its init function
+ * and thus is responsible for freeing it.
+ *
+ * Return: The number of bytes written into the buffer. This is NOT
+ * equal to the size of the SDT, since more space is needed for TS headers during TS
+ * encapsulation.
+ */
+u32 vidtv_psi_sdt_write_into(struct vidtv_psi_sdt_write_args *args);
+
+/**
+ * struct vidtv_psi_pmt_write_args - Arguments for writing a PMT section
+ * @buf: The destination buffer.
+ * @offset: The offset into the destination buffer.
+ * @pmt: A pointer to the PMT.
+ * @pid: Program ID
+ * @buf_sz: The size of the destination buffer.
+ * @continuity_counter: A pointer to the CC. Incremented on every new packet.
+ * @pcr_pid: The TS PID used for the PSI packets. All channels will share the
+ * same PCR.
+ */
+struct vidtv_psi_pmt_write_args {
+ char *buf;
+ u32 offset;
+ struct vidtv_psi_table_pmt *pmt;
+ u16 pid;
+ u32 buf_sz;
+ u8 *continuity_counter;
+ u16 pcr_pid;
+};
+
+/**
+ * vidtv_psi_pmt_write_into - Write PMT as MPEG-TS packets into a buffer.
+ * @args: an instance of struct vidtv_psi_pmt_write_args
+ *
+ * This function writes the MPEG TS packets for a PMT section into a buffer.
+ * Calling code will usually generate the PMT section via a call to its init function
+ * and thus is responsible for freeing it.
+ *
+ * Return: The number of bytes written into the buffer. This is NOT
+ * equal to the size of the PMT section, since more space is needed for TS headers
+ * during TS encapsulation.
+ */
+u32 vidtv_psi_pmt_write_into(struct vidtv_psi_pmt_write_args *args);
+
+/**
+ * vidtv_psi_find_pmt_sec - Finds the PMT section for 'program_num'
+ * @pmt_sections: The sections to look into.
+ * @nsections: The number of sections.
+ * @program_num: The 'program_num' from PAT pointing to a PMT section.
+ *
+ * Return: A pointer to the PMT, if found, or NULL.
+ */
+struct vidtv_psi_table_pmt *vidtv_psi_find_pmt_sec(struct vidtv_psi_table_pmt **pmt_sections,
+ u16 nsections,
+ u16 program_num);
+
+u16 vidtv_psi_get_pat_program_pid(struct vidtv_psi_table_pat_program *p);
+u16 vidtv_psi_pmt_stream_get_elem_pid(struct vidtv_psi_table_pmt_stream *s);
+
+/**
+ * struct vidtv_psi_table_transport - A entry in the TS loop for the NIT and/or other tables.
+ * See ETSI 300 468 section 5.2.1
+ * @transport_id: The TS ID being described
+ * @network_id: The network_id that contains the TS ID
+ * @bitfield: Contains the descriptor loop length
+ * @descriptor: A descriptor loop
+ * @next: Pointer to the next entry
+ *
+ */
+struct vidtv_psi_table_transport {
+ __be16 transport_id;
+ __be16 network_id;
+ __be16 bitfield; /* desc_len: 12, reserved: 4 */
+ struct vidtv_psi_desc *descriptor;
+ struct vidtv_psi_table_transport *next;
+} __packed;
+
+/**
+ * struct vidtv_psi_table_nit - A Network Information Table (NIT). See ETSI 300
+ * 468 section 5.2.1
+ * @header: A PSI table header
+ * @bitfield: Contains the network descriptor length
+ * @descriptor: A descriptor loop describing the network
+ * @bitfield2: Contains the transport stream loop length
+ * @transport: The transport stream loop
+ *
+ */
+struct vidtv_psi_table_nit {
+ struct vidtv_psi_table_header header;
+ __be16 bitfield; /* network_desc_len: 12, reserved:4 */
+ struct vidtv_psi_desc *descriptor;
+ __be16 bitfield2; /* ts_loop_len: 12, reserved: 4 */
+ struct vidtv_psi_table_transport *transport;
+} __packed;
+
+struct vidtv_psi_table_nit
+*vidtv_psi_nit_table_init(u16 network_id,
+ u16 transport_stream_id,
+ char *network_name,
+ struct vidtv_psi_desc_service_list_entry *service_list);
+
+/**
+ * struct vidtv_psi_nit_write_args - Arguments for writing a NIT section
+ * @buf: The destination buffer.
+ * @offset: The offset into the destination buffer.
+ * @nit: A pointer to the NIT
+ * @buf_sz: The size of the destination buffer.
+ * @continuity_counter: A pointer to the CC. Incremented on every new packet.
+ *
+ */
+struct vidtv_psi_nit_write_args {
+ char *buf;
+ u32 offset;
+ struct vidtv_psi_table_nit *nit;
+ u32 buf_sz;
+ u8 *continuity_counter;
+};
+
+/**
+ * vidtv_psi_nit_write_into - Write NIT as MPEG-TS packets into a buffer.
+ * @args: an instance of struct vidtv_psi_nit_write_args
+ *
+ * This function writes the MPEG TS packets for a NIT table into a buffer.
+ * Calling code will usually generate the NIT via a call to its init function
+ * and thus is responsible for freeing it.
+ *
+ * Return: The number of bytes written into the buffer. This is NOT
+ * equal to the size of the NIT, since more space is needed for TS headers during TS
+ * encapsulation.
+ */
+u32 vidtv_psi_nit_write_into(struct vidtv_psi_nit_write_args *args);
+
+void vidtv_psi_nit_table_destroy(struct vidtv_psi_table_nit *nit);
+
+/*
+ * struct vidtv_psi_desc_short_event - A short event descriptor
+ * see ETSI EN 300 468 v1.15.1 section 6.2.37
+ */
+struct vidtv_psi_table_eit_event {
+ __be16 event_id;
+ u8 start_time[5];
+ u8 duration[3];
+ __be16 bitfield; /* desc_length: 12, free_CA_mode: 1, running_status: 1 */
+ struct vidtv_psi_desc *descriptor;
+ struct vidtv_psi_table_eit_event *next;
+} __packed;
+
+/*
+ * struct vidtv_psi_table_eit - A Event Information Table (EIT)
+ * See ETSI 300 468 section 5.2.4
+ */
+struct vidtv_psi_table_eit {
+ struct vidtv_psi_table_header header;
+ __be16 transport_id;
+ __be16 network_id;
+ u8 last_segment;
+ u8 last_table_id;
+ struct vidtv_psi_table_eit_event *event;
+} __packed;
+
+struct vidtv_psi_table_eit
+*vidtv_psi_eit_table_init(u16 network_id,
+ u16 transport_stream_id,
+ __be16 service_id);
+
+/**
+ * struct vidtv_psi_eit_write_args - Arguments for writing an EIT section
+ * @buf: The destination buffer.
+ * @offset: The offset into the destination buffer.
+ * @eit: A pointer to the EIT
+ * @buf_sz: The size of the destination buffer.
+ * @continuity_counter: A pointer to the CC. Incremented on every new packet.
+ *
+ */
+struct vidtv_psi_eit_write_args {
+ char *buf;
+ u32 offset;
+ struct vidtv_psi_table_eit *eit;
+ u32 buf_sz;
+ u8 *continuity_counter;
+};
+
+/**
+ * vidtv_psi_eit_write_into - Write EIT as MPEG-TS packets into a buffer.
+ * @args: an instance of struct vidtv_psi_nit_write_args
+ *
+ * This function writes the MPEG TS packets for a EIT table into a buffer.
+ * Calling code will usually generate the EIT via a call to its init function
+ * and thus is responsible for freeing it.
+ *
+ * Return: The number of bytes written into the buffer. This is NOT
+ * equal to the size of the EIT, since more space is needed for TS headers during TS
+ * encapsulation.
+ */
+u32 vidtv_psi_eit_write_into(struct vidtv_psi_eit_write_args *args);
+
+void vidtv_psi_eit_table_destroy(struct vidtv_psi_table_eit *eit);
+
+/**
+ * vidtv_psi_eit_table_update_sec_len - Recompute and update the EIT section length.
+ * @eit: The EIT whose length is to be updated.
+ *
+ * This will traverse the table and accumulate the length of its components,
+ * which is then used to replace the 'section_length' field.
+ *
+ * If section_length > EIT_MAX_SECTION_LEN, the operation fails.
+ */
+void vidtv_psi_eit_table_update_sec_len(struct vidtv_psi_table_eit *eit);
+
+/**
+ * vidtv_psi_eit_event_assign - Assigns the event loop to the EIT.
+ * @eit: The EIT to assign to.
+ * @e: The event loop
+ *
+ * This will free the previous event loop in the table.
+ * This will assign ownership of the stream loop to the table, i.e. the table
+ * will free this stream loop when a call to its destroy function is made.
+ */
+void vidtv_psi_eit_event_assign(struct vidtv_psi_table_eit *eit,
+ struct vidtv_psi_table_eit_event *e);
+
+struct vidtv_psi_table_eit_event
+*vidtv_psi_eit_event_init(struct vidtv_psi_table_eit_event *head, u16 event_id);
+
+void vidtv_psi_eit_event_destroy(struct vidtv_psi_table_eit_event *e);
+
+#endif // VIDTV_PSI_H
diff --git a/drivers/media/test-drivers/vidtv/vidtv_s302m.c b/drivers/media/test-drivers/vidtv/vidtv_s302m.c
new file mode 100644
index 0000000..d79b658
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_s302m.c
@@ -0,0 +1,515 @@
+// 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 code for an AES3 (also known as AES/EBU) encoder.
+ * It is based on EBU Tech 3250 and SMPTE 302M technical documents.
+ *
+ * This encoder currently supports 16bit AES3 subframes using 16bit signed
+ * integers.
+ *
+ * Note: AU stands for Access Unit, and AAU stands for Audio Access Unit
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ":%s, %d: " fmt, __func__, __LINE__
+
+#include <linux/bug.h>
+#include <linux/crc32.h>
+#include <linux/fixp-arith.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include <linux/printk.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/string.h>
+#include <linux/types.h>
+#include <linux/vmalloc.h>
+
+#include "vidtv_common.h"
+#include "vidtv_encoder.h"
+#include "vidtv_s302m.h"
+
+#define S302M_SAMPLING_RATE_HZ 48000
+#define PES_PRIVATE_STREAM_1 0xbd /* PES: private_stream_1 */
+#define S302M_BLOCK_SZ 192
+#define S302M_SIN_LUT_NUM_ELEM 1024
+
+/* these are retrieved empirically from ffmpeg/libavcodec */
+#define FF_S302M_DEFAULT_NUM_FRAMES 1115
+#define FF_S302M_DEFAULT_PTS_INCREMENT 2090
+#define FF_S302M_DEFAULT_PTS_OFFSET 100000
+
+/* Used by the tone generator: number of samples for PI */
+#define PI 180
+
+static const u8 reverse[256] = {
+ /* from ffmpeg */
+ 0x00, 0x80, 0x40, 0xC0, 0x20, 0xA0, 0x60, 0xE0, 0x10, 0x90, 0x50, 0xD0,
+ 0x30, 0xB0, 0x70, 0xF0, 0x08, 0x88, 0x48, 0xC8, 0x28, 0xA8, 0x68, 0xE8,
+ 0x18, 0x98, 0x58, 0xD8, 0x38, 0xB8, 0x78, 0xF8, 0x04, 0x84, 0x44, 0xC4,
+ 0x24, 0xA4, 0x64, 0xE4, 0x14, 0x94, 0x54, 0xD4, 0x34, 0xB4, 0x74, 0xF4,
+ 0x0C, 0x8C, 0x4C, 0xCC, 0x2C, 0xAC, 0x6C, 0xEC, 0x1C, 0x9C, 0x5C, 0xDC,
+ 0x3C, 0xBC, 0x7C, 0xFC, 0x02, 0x82, 0x42, 0xC2, 0x22, 0xA2, 0x62, 0xE2,
+ 0x12, 0x92, 0x52, 0xD2, 0x32, 0xB2, 0x72, 0xF2, 0x0A, 0x8A, 0x4A, 0xCA,
+ 0x2A, 0xAA, 0x6A, 0xEA, 0x1A, 0x9A, 0x5A, 0xDA, 0x3A, 0xBA, 0x7A, 0xFA,
+ 0x06, 0x86, 0x46, 0xC6, 0x26, 0xA6, 0x66, 0xE6, 0x16, 0x96, 0x56, 0xD6,
+ 0x36, 0xB6, 0x76, 0xF6, 0x0E, 0x8E, 0x4E, 0xCE, 0x2E, 0xAE, 0x6E, 0xEE,
+ 0x1E, 0x9E, 0x5E, 0xDE, 0x3E, 0xBE, 0x7E, 0xFE, 0x01, 0x81, 0x41, 0xC1,
+ 0x21, 0xA1, 0x61, 0xE1, 0x11, 0x91, 0x51, 0xD1, 0x31, 0xB1, 0x71, 0xF1,
+ 0x09, 0x89, 0x49, 0xC9, 0x29, 0xA9, 0x69, 0xE9, 0x19, 0x99, 0x59, 0xD9,
+ 0x39, 0xB9, 0x79, 0xF9, 0x05, 0x85, 0x45, 0xC5, 0x25, 0xA5, 0x65, 0xE5,
+ 0x15, 0x95, 0x55, 0xD5, 0x35, 0xB5, 0x75, 0xF5, 0x0D, 0x8D, 0x4D, 0xCD,
+ 0x2D, 0xAD, 0x6D, 0xED, 0x1D, 0x9D, 0x5D, 0xDD, 0x3D, 0xBD, 0x7D, 0xFD,
+ 0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3, 0x13, 0x93, 0x53, 0xD3,
+ 0x33, 0xB3, 0x73, 0xF3, 0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB,
+ 0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB, 0x07, 0x87, 0x47, 0xC7,
+ 0x27, 0xA7, 0x67, 0xE7, 0x17, 0x97, 0x57, 0xD7, 0x37, 0xB7, 0x77, 0xF7,
+ 0x0F, 0x8F, 0x4F, 0xCF, 0x2F, 0xAF, 0x6F, 0xEF, 0x1F, 0x9F, 0x5F, 0xDF,
+ 0x3F, 0xBF, 0x7F, 0xFF,
+};
+
+struct tone_duration {
+ enum musical_notes note;
+ int duration;
+};
+
+#define COMPASS 100 /* beats per minute */
+static const struct tone_duration beethoven_fur_elise[] = {
+ { NOTE_SILENT, 512},
+ { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
+ { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
+ { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
+ { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
+ { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
+ { NOTE_GS_5, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_E_5, 128},
+ { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
+ { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
+ { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
+ { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
+ { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
+ { NOTE_C_6, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_SILENT, 128},
+
+ { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
+ { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
+ { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
+ { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
+ { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
+ { NOTE_GS_5, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_E_5, 128},
+ { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
+ { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
+ { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
+ { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
+ { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
+ { NOTE_C_6, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_B_4, 128},
+ { NOTE_C_5, 128}, { NOTE_D_5, 128}, { NOTE_C_4, 128},
+ { NOTE_G_4, 128}, { NOTE_C_5, 128}, { NOTE_G_4, 128},
+ { NOTE_F_5, 128}, { NOTE_E_5, 128}, { NOTE_G_3, 128},
+ { NOTE_G_4, 128}, { NOTE_B_3, 128}, { NOTE_F_4, 128},
+ { NOTE_E_5, 128}, { NOTE_D_5, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_E_4, 128},
+ { NOTE_D_5, 128}, { NOTE_C_5, 128}, { NOTE_E_3, 128},
+ { NOTE_E_4, 128}, { NOTE_E_5, 128}, { NOTE_E_5, 128},
+ { NOTE_E_6, 128}, { NOTE_E_5, 128}, { NOTE_E_6, 128},
+ { NOTE_E_5, 128}, { NOTE_E_5, 128}, { NOTE_DS_5, 128},
+ { NOTE_E_5, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
+ { NOTE_DS_5, 128}, { NOTE_E_5, 128}, { NOTE_DS_6, 128},
+ { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
+ { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
+ { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
+ { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
+ { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
+ { NOTE_GS_5, 128}, { NOTE_B_5, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_E_5, 128},
+ { NOTE_E_6, 128}, { NOTE_DS_6, 128}, { NOTE_E_6, 128},
+ { NOTE_DS_6, 128}, { NOTE_E_6, 128}, { NOTE_B_5, 128},
+ { NOTE_D_6, 128}, { NOTE_C_6, 128}, { NOTE_A_3, 128},
+ { NOTE_E_4, 128}, { NOTE_A_4, 128}, { NOTE_C_5, 128},
+ { NOTE_E_5, 128}, { NOTE_A_5, 128}, { NOTE_E_3, 128},
+ { NOTE_E_4, 128}, { NOTE_GS_4, 128}, { NOTE_E_5, 128},
+ { NOTE_C_6, 128}, { NOTE_B_5, 128}, { NOTE_A_5, 512},
+ { NOTE_SILENT, 256},
+};
+
+static struct vidtv_access_unit *vidtv_s302m_access_unit_init(struct vidtv_access_unit *head)
+{
+ struct vidtv_access_unit *au;
+
+ au = kzalloc(sizeof(*au), GFP_KERNEL);
+ if (!au)
+ return NULL;
+
+ if (head) {
+ while (head->next)
+ head = head->next;
+
+ head->next = au;
+ }
+
+ return au;
+}
+
+static void vidtv_s302m_access_unit_destroy(struct vidtv_encoder *e)
+{
+ struct vidtv_access_unit *head = e->access_units;
+ struct vidtv_access_unit *tmp = NULL;
+
+ while (head) {
+ tmp = head;
+ head = head->next;
+ kfree(tmp);
+ }
+
+ e->access_units = NULL;
+}
+
+static void vidtv_s302m_alloc_au(struct vidtv_encoder *e)
+{
+ struct vidtv_access_unit *sync_au = NULL;
+ struct vidtv_access_unit *temp = NULL;
+
+ if (e->sync && e->sync->is_video_encoder) {
+ sync_au = e->sync->access_units;
+
+ while (sync_au) {
+ temp = vidtv_s302m_access_unit_init(e->access_units);
+ if (!e->access_units)
+ e->access_units = temp;
+
+ sync_au = sync_au->next;
+ }
+
+ return;
+ }
+
+ e->access_units = vidtv_s302m_access_unit_init(NULL);
+}
+
+static void
+vidtv_s302m_compute_sample_count_from_video(struct vidtv_encoder *e)
+{
+ struct vidtv_access_unit *sync_au = e->sync->access_units;
+ struct vidtv_access_unit *au = e->access_units;
+ u32 sample_duration_usecs;
+ u32 vau_duration_usecs;
+ u32 s;
+
+ vau_duration_usecs = USEC_PER_SEC / e->sync->sampling_rate_hz;
+ sample_duration_usecs = USEC_PER_SEC / e->sampling_rate_hz;
+
+ while (au && sync_au) {
+ s = DIV_ROUND_UP(vau_duration_usecs, sample_duration_usecs);
+ au->num_samples = s;
+ au = au->next;
+ sync_au = sync_au->next;
+ }
+}
+
+static void vidtv_s302m_compute_pts_from_video(struct vidtv_encoder *e)
+{
+ struct vidtv_access_unit *au = e->access_units;
+ struct vidtv_access_unit *sync_au = e->sync->access_units;
+
+ /* use the same pts from the video access unit*/
+ while (au && sync_au) {
+ au->pts = sync_au->pts;
+ au = au->next;
+ sync_au = sync_au->next;
+ }
+}
+
+static u16 vidtv_s302m_get_sample(struct vidtv_encoder *e)
+{
+ u16 sample;
+ int pos;
+ struct vidtv_s302m_ctx *ctx = e->ctx;
+
+ if (!e->src_buf) {
+ /*
+ * Simple tone generator: play the tones at the
+ * beethoven_fur_elise array.
+ */
+ if (ctx->last_duration <= 0) {
+ if (e->src_buf_offset >= ARRAY_SIZE(beethoven_fur_elise))
+ e->src_buf_offset = 0;
+
+ ctx->last_tone = beethoven_fur_elise[e->src_buf_offset].note;
+ ctx->last_duration = beethoven_fur_elise[e->src_buf_offset].duration *
+ S302M_SAMPLING_RATE_HZ / COMPASS / 5;
+ e->src_buf_offset++;
+ ctx->note_offset = 0;
+ } else {
+ ctx->last_duration--;
+ }
+
+ /* Handle pause notes */
+ if (!ctx->last_tone)
+ return 0x8000;
+
+ pos = (2 * PI * ctx->note_offset * ctx->last_tone) / S302M_SAMPLING_RATE_HZ;
+ ctx->note_offset++;
+
+ return (fixp_sin32(pos % (2 * PI)) >> 16) + 0x8000;
+ }
+
+ /* bug somewhere */
+ if (e->src_buf_offset > e->src_buf_sz) {
+ pr_err_ratelimited("overflow detected: %d > %d, wrapping.\n",
+ e->src_buf_offset,
+ e->src_buf_sz);
+
+ e->src_buf_offset = 0;
+ }
+
+ if (e->src_buf_offset >= e->src_buf_sz) {
+ /* let the source know we are out of data */
+ if (e->last_sample_cb)
+ e->last_sample_cb(e->sample_count);
+
+ e->src_buf_offset = 0;
+ }
+
+ sample = *(u16 *)(e->src_buf + e->src_buf_offset);
+
+ return sample;
+}
+
+static u32 vidtv_s302m_write_frame(struct vidtv_encoder *e,
+ u16 sample)
+{
+ struct vidtv_s302m_ctx *ctx = e->ctx;
+ struct vidtv_s302m_frame_16 f = {};
+ u32 nbytes = 0;
+
+ /* from ffmpeg: see s302enc.c */
+
+ u8 vucf = ctx->frame_index == 0 ? 0x10 : 0;
+
+ f.data[0] = sample & 0xFF;
+ f.data[1] = (sample & 0xFF00) >> 8;
+ f.data[2] = ((sample & 0x0F) << 4) | vucf;
+ f.data[3] = (sample & 0x0FF0) >> 4;
+ f.data[4] = (sample & 0xF000) >> 12;
+
+ f.data[0] = reverse[f.data[0]];
+ f.data[1] = reverse[f.data[1]];
+ f.data[2] = reverse[f.data[2]];
+ f.data[3] = reverse[f.data[3]];
+ f.data[4] = reverse[f.data[4]];
+
+ nbytes += vidtv_memcpy(e->encoder_buf,
+ e->encoder_buf_offset,
+ VIDTV_S302M_BUF_SZ,
+ &f,
+ sizeof(f));
+
+ e->encoder_buf_offset += nbytes;
+
+ ctx->frame_index++;
+ if (ctx->frame_index >= S302M_BLOCK_SZ)
+ ctx->frame_index = 0;
+
+ return nbytes;
+}
+
+static u32 vidtv_s302m_write_h(struct vidtv_encoder *e, u32 p_sz)
+{
+ struct vidtv_smpte_s302m_es h = {};
+ u32 nbytes = 0;
+
+ /* 2 channels, ident: 0, 16 bits per sample */
+ h.bitfield = cpu_to_be32((p_sz << 16));
+
+ nbytes += vidtv_memcpy(e->encoder_buf,
+ e->encoder_buf_offset,
+ e->encoder_buf_sz,
+ &h,
+ sizeof(h));
+
+ e->encoder_buf_offset += nbytes;
+ return nbytes;
+}
+
+static void vidtv_s302m_write_frames(struct vidtv_encoder *e)
+{
+ struct vidtv_access_unit *au = e->access_units;
+ struct vidtv_s302m_ctx *ctx = e->ctx;
+ u32 nbytes_per_unit = 0;
+ u32 nbytes = 0;
+ u32 au_sz = 0;
+ u16 sample;
+ u32 j;
+
+ while (au) {
+ au_sz = au->num_samples *
+ sizeof(struct vidtv_s302m_frame_16);
+
+ nbytes_per_unit = vidtv_s302m_write_h(e, au_sz);
+
+ for (j = 0; j < au->num_samples; ++j) {
+ sample = vidtv_s302m_get_sample(e);
+ nbytes_per_unit += vidtv_s302m_write_frame(e, sample);
+
+ if (e->src_buf)
+ e->src_buf_offset += sizeof(u16);
+
+ e->sample_count++;
+ }
+
+ au->nbytes = nbytes_per_unit;
+
+ if (au_sz + sizeof(struct vidtv_smpte_s302m_es) != nbytes_per_unit) {
+ pr_warn_ratelimited("write size was %u, expected %zu\n",
+ nbytes_per_unit,
+ au_sz + sizeof(struct vidtv_smpte_s302m_es));
+ }
+
+ nbytes += nbytes_per_unit;
+ au->offset = nbytes - nbytes_per_unit;
+
+ nbytes_per_unit = 0;
+ ctx->au_count++;
+
+ au = au->next;
+ }
+}
+
+static void *vidtv_s302m_encode(struct vidtv_encoder *e)
+{
+ struct vidtv_s302m_ctx *ctx = e->ctx;
+
+ /*
+ * According to SMPTE 302M, an audio access unit is specified as those
+ * AES3 words that are associated with a corresponding video frame.
+ * Therefore, there is one audio access unit for every video access unit
+ * in the corresponding video encoder ('sync'), using the same values
+ * for PTS as used by the video encoder.
+ *
+ * Assuming that it is also possible to send audio without any
+ * associated video, as in a radio-like service, a single audio access unit
+ * is created with values for 'num_samples' and 'pts' taken empirically from
+ * ffmpeg
+ */
+
+ vidtv_s302m_access_unit_destroy(e);
+ vidtv_s302m_alloc_au(e);
+
+ if (e->sync && e->sync->is_video_encoder) {
+ vidtv_s302m_compute_sample_count_from_video(e);
+ vidtv_s302m_compute_pts_from_video(e);
+ } else {
+ e->access_units->num_samples = FF_S302M_DEFAULT_NUM_FRAMES;
+ e->access_units->pts = (ctx->au_count * FF_S302M_DEFAULT_PTS_INCREMENT) +
+ FF_S302M_DEFAULT_PTS_OFFSET;
+ }
+
+ vidtv_s302m_write_frames(e);
+
+ return e->encoder_buf;
+}
+
+static u32 vidtv_s302m_clear(struct vidtv_encoder *e)
+{
+ struct vidtv_access_unit *au = e->access_units;
+ u32 count = 0;
+
+ while (au) {
+ count++;
+ au = au->next;
+ }
+
+ vidtv_s302m_access_unit_destroy(e);
+ memset(e->encoder_buf, 0, VIDTV_S302M_BUF_SZ);
+ e->encoder_buf_offset = 0;
+
+ return count;
+}
+
+struct vidtv_encoder
+*vidtv_s302m_encoder_init(struct vidtv_s302m_encoder_init_args args)
+{
+ u32 priv_sz = sizeof(struct vidtv_s302m_ctx);
+ struct vidtv_s302m_ctx *ctx;
+ struct vidtv_encoder *e;
+
+ e = kzalloc(sizeof(*e), GFP_KERNEL);
+ if (!e)
+ return NULL;
+
+ e->id = S302M;
+
+ if (args.name)
+ e->name = kstrdup(args.name, GFP_KERNEL);
+
+ e->encoder_buf = vzalloc(VIDTV_S302M_BUF_SZ);
+ e->encoder_buf_sz = VIDTV_S302M_BUF_SZ;
+ e->encoder_buf_offset = 0;
+
+ e->sample_count = 0;
+
+ e->src_buf = (args.src_buf) ? args.src_buf : NULL;
+ e->src_buf_sz = (args.src_buf) ? args.src_buf_sz : 0;
+ e->src_buf_offset = 0;
+
+ e->is_video_encoder = false;
+
+ ctx = kzalloc(priv_sz, GFP_KERNEL);
+ if (!ctx) {
+ kfree(e);
+ return NULL;
+ }
+
+ e->ctx = ctx;
+ ctx->last_duration = 0;
+
+ e->encode = vidtv_s302m_encode;
+ e->clear = vidtv_s302m_clear;
+
+ e->es_pid = cpu_to_be16(args.es_pid);
+ e->stream_id = cpu_to_be16(PES_PRIVATE_STREAM_1);
+
+ e->sync = args.sync;
+ e->sampling_rate_hz = S302M_SAMPLING_RATE_HZ;
+
+ e->last_sample_cb = args.last_sample_cb;
+
+ e->destroy = vidtv_s302m_encoder_destroy;
+
+ if (args.head) {
+ while (args.head->next)
+ args.head = args.head->next;
+
+ args.head->next = e;
+ }
+
+ e->next = NULL;
+
+ return e;
+}
+
+void vidtv_s302m_encoder_destroy(struct vidtv_encoder *e)
+{
+ if (e->id != S302M) {
+ pr_err_ratelimited("Encoder type mismatch, skipping.\n");
+ return;
+ }
+
+ vidtv_s302m_access_unit_destroy(e);
+ kfree(e->name);
+ vfree(e->encoder_buf);
+ kfree(e->ctx);
+ kfree(e);
+}
diff --git a/drivers/media/test-drivers/vidtv/vidtv_s302m.h b/drivers/media/test-drivers/vidtv/vidtv_s302m.h
new file mode 100644
index 0000000..9cc94e4
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_s302m.h
@@ -0,0 +1,97 @@
+/* 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 code for an AES3 (also known as AES/EBU) encoder.
+ * It is based on EBU Tech 3250 and SMPTE 302M technical documents.
+ *
+ * This encoder currently supports 16bit AES3 subframes using 16bit signed
+ * integers.
+ *
+ * Note: AU stands for Access Unit, and AAU stands for Audio Access Unit
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#ifndef VIDTV_S302M_H
+#define VIDTV_S302M_H
+
+#include <linux/types.h>
+
+#include "vidtv_encoder.h"
+
+/* see SMPTE 302M 2007 clause 7.3 */
+#define VIDTV_S302M_BUF_SZ 65024
+
+/* see ETSI TS 102 154 v.1.2.1 clause 7.3.5 */
+#define VIDTV_S302M_FORMAT_IDENTIFIER 0x42535344
+
+/**
+ * struct vidtv_s302m_ctx - s302m encoder context.
+ * @enc: A pointer to the containing encoder structure.
+ * @frame_index: The current frame in a block
+ * @au_count: The total number of access units encoded up to now
+ * @last_duration: Duration of the tone currently being played
+ * @note_offset: Position at the music tone array
+ * @last_tone: Tone currently being played
+ */
+struct vidtv_s302m_ctx {
+ struct vidtv_encoder *enc;
+ u32 frame_index;
+ u32 au_count;
+ int last_duration;
+ unsigned int note_offset;
+ enum musical_notes last_tone;
+};
+
+/*
+ * struct vidtv_smpte_s302m_es - s302m MPEG Elementary Stream header.
+ *
+ * See SMPTE 302M 2007 table 1.
+ */
+struct vidtv_smpte_s302m_es {
+ /*
+ *
+ * audio_packet_size:16;
+ * num_channels:2;
+ * channel_identification:8;
+ * bits_per_sample:2; // 0x0 for 16bits
+ * zero:4;
+ */
+ __be32 bitfield;
+} __packed;
+
+struct vidtv_s302m_frame_16 {
+ u8 data[5];
+} __packed;
+
+/**
+ * struct vidtv_s302m_encoder_init_args - Args for the s302m encoder.
+ *
+ * @name: A name to identify this particular instance
+ * @src_buf: The source buffer, encoder will default to a sine wave if this is NULL.
+ * @src_buf_sz: The size of the source buffer.
+ * @es_pid: The MPEG Elementary Stream PID to use.
+ * @sync: Attempt to synchronize audio with this video encoder, if not NULL.
+ * @last_sample_cb: A callback called when the encoder runs out of data.
+ * @head: Add to this chain
+ */
+struct vidtv_s302m_encoder_init_args {
+ char *name;
+ void *src_buf;
+ u32 src_buf_sz;
+ u16 es_pid;
+ struct vidtv_encoder *sync;
+ void (*last_sample_cb)(u32 sample_no);
+
+ struct vidtv_encoder *head;
+};
+
+struct vidtv_encoder
+*vidtv_s302m_encoder_init(struct vidtv_s302m_encoder_init_args args);
+
+void vidtv_s302m_encoder_destroy(struct vidtv_encoder *encoder);
+
+#endif /* VIDTV_S302M_H */
diff --git a/drivers/media/test-drivers/vidtv/vidtv_ts.c b/drivers/media/test-drivers/vidtv/vidtv_ts.c
new file mode 100644
index 0000000..ca4bb9c
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_ts.c
@@ -0,0 +1,136 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * The Virtual DVB test driver 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.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#define pr_fmt(fmt) KBUILD_MODNAME ":%s, %d: " fmt, __func__, __LINE__
+
+#include <linux/math64.h>
+#include <linux/printk.h>
+#include <linux/ratelimit.h>
+#include <linux/types.h>
+
+#include "vidtv_common.h"
+#include "vidtv_ts.h"
+
+static u32 vidtv_ts_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;
+}
+
+void vidtv_ts_inc_cc(u8 *continuity_counter)
+{
+ ++*continuity_counter;
+ if (*continuity_counter > TS_CC_MAX_VAL)
+ *continuity_counter = 0;
+}
+
+u32 vidtv_ts_null_write_into(struct null_packet_write_args args)
+{
+ u32 nbytes = 0;
+ struct vidtv_mpeg_ts ts_header = {};
+
+ ts_header.sync_byte = TS_SYNC_BYTE;
+ ts_header.bitfield = cpu_to_be16(TS_NULL_PACKET_PID);
+ ts_header.payload = 1;
+ ts_header.continuity_counter = *args.continuity_counter;
+
+ /* copy TS header */
+ nbytes += vidtv_memcpy(args.dest_buf,
+ args.dest_offset + nbytes,
+ args.buf_sz,
+ &ts_header,
+ sizeof(ts_header));
+
+ vidtv_ts_inc_cc(args.continuity_counter);
+
+ /* fill the rest with empty data */
+ nbytes += vidtv_memset(args.dest_buf,
+ args.dest_offset + nbytes,
+ args.buf_sz,
+ TS_FILL_BYTE,
+ TS_PACKET_LEN - nbytes);
+
+ /* we should have written exactly _one_ 188byte packet */
+ if (nbytes != TS_PACKET_LEN)
+ pr_warn_ratelimited("Expected exactly %d bytes, got %d\n",
+ TS_PACKET_LEN,
+ nbytes);
+
+ return nbytes;
+}
+
+u32 vidtv_ts_pcr_write_into(struct pcr_write_args args)
+{
+ u32 nbytes = 0;
+ struct vidtv_mpeg_ts ts_header = {};
+ struct vidtv_mpeg_ts_adaption ts_adap = {};
+
+ ts_header.sync_byte = TS_SYNC_BYTE;
+ ts_header.bitfield = cpu_to_be16(args.pid);
+ ts_header.scrambling = 0;
+ /* cc is not incremented, but it is needed. see 13818-1 clause 2.4.3.3 */
+ ts_header.continuity_counter = *args.continuity_counter;
+ ts_header.payload = 0;
+ ts_header.adaptation_field = 1;
+
+ /* 13818-1 clause 2.4.3.5 */
+ ts_adap.length = 183;
+ ts_adap.PCR = 1;
+
+ /* copy TS header */
+ nbytes += vidtv_memcpy(args.dest_buf,
+ args.dest_offset + nbytes,
+ args.buf_sz,
+ &ts_header,
+ sizeof(ts_header));
+
+ /* write the adap after the TS header */
+ nbytes += vidtv_memcpy(args.dest_buf,
+ args.dest_offset + nbytes,
+ args.buf_sz,
+ &ts_adap,
+ sizeof(ts_adap));
+
+ /* write the PCR optional */
+ nbytes += vidtv_ts_write_pcr_bits(args.dest_buf,
+ args.dest_offset + nbytes,
+ args.pcr);
+
+ nbytes += vidtv_memset(args.dest_buf,
+ args.dest_offset + nbytes,
+ args.buf_sz,
+ TS_FILL_BYTE,
+ TS_PACKET_LEN - nbytes);
+
+ /* we should have written exactly _one_ 188byte packet */
+ if (nbytes != TS_PACKET_LEN)
+ pr_warn_ratelimited("Expected exactly %d bytes, got %d\n",
+ TS_PACKET_LEN,
+ nbytes);
+
+ return nbytes;
+}
diff --git a/drivers/media/test-drivers/vidtv/vidtv_ts.h b/drivers/media/test-drivers/vidtv/vidtv_ts.h
new file mode 100644
index 0000000..f5e8e1f
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_ts.h
@@ -0,0 +1,107 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * The Virtual DVB test driver 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.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#ifndef VIDTV_TS_H
+#define VIDTV_TS_H
+
+#include <linux/types.h>
+
+#define TS_SYNC_BYTE 0x47
+#define TS_PACKET_LEN 188
+#define TS_PAYLOAD_LEN 184
+#define TS_NULL_PACKET_PID 0x1fff
+#define TS_CC_MAX_VAL 0x0f /* 4 bits */
+#define TS_LAST_VALID_PID 8191
+#define TS_FILL_BYTE 0xff /* the byte used in packet stuffing */
+
+struct vidtv_mpeg_ts_adaption {
+ u8 length;
+ struct {
+ u8 extension:1;
+ u8 private_data:1;
+ u8 splicing_point:1;
+ u8 OPCR:1;
+ u8 PCR:1;
+ u8 priority:1;
+ u8 random_access:1;
+ u8 discontinued:1;
+ } __packed;
+ u8 data[];
+} __packed;
+
+struct vidtv_mpeg_ts {
+ u8 sync_byte;
+ __be16 bitfield; /* tei: 1, payload_start:1 priority: 1, pid:13 */
+ struct {
+ u8 continuity_counter:4;
+ u8 payload:1;
+ u8 adaptation_field:1;
+ u8 scrambling:2;
+ } __packed;
+ struct vidtv_mpeg_ts_adaption *adaption;
+} __packed;
+
+/**
+ * struct pcr_write_args - Arguments for the pcr_write_into function.
+ * @dest_buf: The buffer to write into.
+ * @dest_offset: The byte offset into the buffer.
+ * @pid: The TS PID for the PCR packets.
+ * @buf_sz: The size of the buffer in bytes.
+ * @continuity_counter: The TS continuity_counter.
+ * @pcr: A sample from the system clock.
+ */
+struct pcr_write_args {
+ void *dest_buf;
+ u32 dest_offset;
+ u16 pid;
+ u32 buf_sz;
+ u8 *continuity_counter;
+ u64 pcr;
+};
+
+/**
+ * struct null_packet_write_args - Arguments for the null_write_into function
+ * @dest_buf: The buffer to write into.
+ * @dest_offset: The byte offset into the buffer.
+ * @buf_sz: The size of the buffer in bytes.
+ * @continuity_counter: The TS continuity_counter.
+ */
+struct null_packet_write_args {
+ void *dest_buf;
+ u32 dest_offset;
+ u32 buf_sz;
+ u8 *continuity_counter;
+};
+
+/* Increment the continuity counter */
+void vidtv_ts_inc_cc(u8 *continuity_counter);
+
+/**
+ * vidtv_ts_null_write_into - Write a TS null packet into a buffer.
+ * @args: the arguments to use when writing.
+ *
+ * This function will write a null packet into a buffer. This is usually used to
+ * pad TS streams.
+ *
+ * Return: The number of bytes written into the buffer.
+ */
+u32 vidtv_ts_null_write_into(struct null_packet_write_args args);
+
+/**
+ * vidtv_ts_pcr_write_into - Write a PCR packet into a buffer.
+ * @args: the arguments to use when writing.
+ *
+ * This function will write a PCR packet into a buffer. This is used to
+ * synchronize the clocks between encoders and decoders.
+ *
+ * Return: The number of bytes written into the buffer.
+ */
+u32 vidtv_ts_pcr_write_into(struct pcr_write_args args);
+
+#endif //VIDTV_TS_H
diff --git a/drivers/media/test-drivers/vidtv/vidtv_tuner.c b/drivers/media/test-drivers/vidtv/vidtv_tuner.c
new file mode 100644
index 0000000..14b6bc9
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_tuner.c
@@ -0,0 +1,439 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * The Virtual DVB test driver 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.
+ *
+ * The vidtv tuner should support common TV standards such as
+ * DVB-T/T2/S/S2, ISDB-T and ATSC when completed.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#include <linux/errno.h>
+#include <linux/i2c.h>
+#include <linux/module.h>
+#include <linux/printk.h>
+#include <linux/ratelimit.h>
+#include <linux/slab.h>
+#include <linux/types.h>
+
+#include <media/dvb_frontend.h>
+
+#include "vidtv_tuner.h"
+
+struct vidtv_tuner_cnr_to_qual_s {
+ /* attempt to use the same values as libdvbv5 */
+ u32 modulation;
+ u32 fec;
+ u32 cnr_ok;
+ u32 cnr_good;
+};
+
+static const struct vidtv_tuner_cnr_to_qual_s vidtv_tuner_c_cnr_2_qual[] = {
+ /* from libdvbv5 source code, in milli db */
+ { QAM_256, FEC_NONE, 34000, 38000},
+ { QAM_64, FEC_NONE, 30000, 34000},
+};
+
+static const struct vidtv_tuner_cnr_to_qual_s vidtv_tuner_s_cnr_2_qual[] = {
+ /* from libdvbv5 source code, in milli db */
+ { QPSK, FEC_1_2, 7000, 10000},
+ { QPSK, FEC_2_3, 9000, 12000},
+ { QPSK, FEC_3_4, 10000, 13000},
+ { QPSK, FEC_5_6, 11000, 14000},
+ { QPSK, FEC_7_8, 12000, 15000},
+};
+
+static const struct vidtv_tuner_cnr_to_qual_s vidtv_tuner_s2_cnr_2_qual[] = {
+ /* from libdvbv5 source code, in milli db */
+ { QPSK, FEC_1_2, 9000, 12000},
+ { QPSK, FEC_2_3, 11000, 14000},
+ { QPSK, FEC_3_4, 12000, 15000},
+ { QPSK, FEC_5_6, 12000, 15000},
+ { QPSK, FEC_8_9, 13000, 16000},
+ { QPSK, FEC_9_10, 13500, 16500},
+ { PSK_8, FEC_2_3, 14500, 17500},
+ { PSK_8, FEC_3_4, 16000, 19000},
+ { PSK_8, FEC_5_6, 17500, 20500},
+ { PSK_8, FEC_8_9, 19000, 22000},
+};
+
+static const struct vidtv_tuner_cnr_to_qual_s vidtv_tuner_t_cnr_2_qual[] = {
+ /* from libdvbv5 source code, in milli db*/
+ { QPSK, FEC_1_2, 4100, 5900},
+ { QPSK, FEC_2_3, 6100, 9600},
+ { QPSK, FEC_3_4, 7200, 12400},
+ { QPSK, FEC_5_6, 8500, 15600},
+ { QPSK, FEC_7_8, 9200, 17500},
+ { QAM_16, FEC_1_2, 9800, 11800},
+ { QAM_16, FEC_2_3, 12100, 15300},
+ { QAM_16, FEC_3_4, 13400, 18100},
+ { QAM_16, FEC_5_6, 14800, 21300},
+ { QAM_16, FEC_7_8, 15700, 23600},
+ { QAM_64, FEC_1_2, 14000, 16000},
+ { QAM_64, FEC_2_3, 19900, 25400},
+ { QAM_64, FEC_3_4, 24900, 27900},
+ { QAM_64, FEC_5_6, 21300, 23300},
+ { QAM_64, FEC_7_8, 22000, 24000},
+};
+
+/**
+ * struct vidtv_tuner_hardware_state - Simulate the tuner hardware status
+ * @asleep: whether the tuner is asleep, i.e whether _sleep() or _suspend() was
+ * called.
+ * @lock_status: Whether the tuner has managed to lock on the requested
+ * frequency.
+ * @if_frequency: The tuner's intermediate frequency. Hardcoded for the purposes
+ * of simulation.
+ * @tuned_frequency: The actual tuned frequency.
+ * @bandwidth: The actual bandwidth.
+ *
+ * This structure is meant to simulate the status of the tuner hardware, as if
+ * we had a physical tuner hardware.
+ */
+struct vidtv_tuner_hardware_state {
+ bool asleep;
+ u32 lock_status;
+ u32 if_frequency;
+ u32 tuned_frequency;
+ u32 bandwidth;
+};
+
+/**
+ * struct vidtv_tuner_dev - The tuner struct
+ * @fe: A pointer to the dvb_frontend structure allocated by vidtv_demod
+ * @hw_state: A struct to simulate the tuner's hardware state as if we had a
+ * physical tuner hardware.
+ * @config: The configuration used to start the tuner module, usually filled
+ * by a bridge driver. For vidtv, this is filled by vidtv_bridge before the
+ * tuner module is probed.
+ */
+struct vidtv_tuner_dev {
+ struct dvb_frontend *fe;
+ struct vidtv_tuner_hardware_state hw_state;
+ struct vidtv_tuner_config config;
+};
+
+static struct vidtv_tuner_dev*
+vidtv_tuner_get_dev(struct dvb_frontend *fe)
+{
+ return i2c_get_clientdata(fe->tuner_priv);
+}
+
+static int vidtv_tuner_check_frequency_shift(struct dvb_frontend *fe)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+ struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+ struct vidtv_tuner_config config = tuner_dev->config;
+ u32 *valid_freqs = NULL;
+ u32 array_sz = 0;
+ u32 i;
+ u32 shift;
+
+ switch (c->delivery_system) {
+ case SYS_DVBT:
+ case SYS_DVBT2:
+ valid_freqs = config.vidtv_valid_dvb_t_freqs;
+ array_sz = ARRAY_SIZE(config.vidtv_valid_dvb_t_freqs);
+ break;
+ case SYS_DVBS:
+ case SYS_DVBS2:
+ valid_freqs = config.vidtv_valid_dvb_s_freqs;
+ array_sz = ARRAY_SIZE(config.vidtv_valid_dvb_s_freqs);
+ break;
+ case SYS_DVBC_ANNEX_A:
+ valid_freqs = config.vidtv_valid_dvb_c_freqs;
+ array_sz = ARRAY_SIZE(config.vidtv_valid_dvb_c_freqs);
+ break;
+
+ default:
+ dev_warn(fe->dvb->device,
+ "%s: unsupported delivery system: %u\n",
+ __func__,
+ c->delivery_system);
+
+ return -EINVAL;
+ }
+
+ for (i = 0; i < array_sz; i++) {
+ if (!valid_freqs[i])
+ break;
+ shift = abs(c->frequency - valid_freqs[i]);
+
+ if (!shift)
+ return 0;
+
+ /*
+ * This will provide a value from 0 to 100 that would
+ * indicate how far is the tuned frequency from the
+ * right one.
+ */
+ if (shift < config.max_frequency_shift_hz)
+ return shift * 100 / config.max_frequency_shift_hz;
+ }
+
+ return -EINVAL;
+}
+
+static int
+vidtv_tuner_get_signal_strength(struct dvb_frontend *fe, u16 *strength)
+{
+ struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+ const struct vidtv_tuner_cnr_to_qual_s *cnr2qual = NULL;
+ struct device *dev = fe->dvb->device;
+ u32 array_size = 0;
+ s32 shift;
+ u32 i;
+
+ shift = vidtv_tuner_check_frequency_shift(fe);
+ if (shift < 0) {
+ tuner_dev->hw_state.lock_status = 0;
+ *strength = 0;
+ return 0;
+ }
+
+ switch (c->delivery_system) {
+ case SYS_DVBT:
+ case SYS_DVBT2:
+ cnr2qual = vidtv_tuner_t_cnr_2_qual;
+ array_size = ARRAY_SIZE(vidtv_tuner_t_cnr_2_qual);
+ break;
+
+ case SYS_DVBS:
+ cnr2qual = vidtv_tuner_s_cnr_2_qual;
+ array_size = ARRAY_SIZE(vidtv_tuner_s_cnr_2_qual);
+ break;
+
+ case SYS_DVBS2:
+ cnr2qual = vidtv_tuner_s2_cnr_2_qual;
+ array_size = ARRAY_SIZE(vidtv_tuner_s2_cnr_2_qual);
+ break;
+
+ case SYS_DVBC_ANNEX_A:
+ cnr2qual = vidtv_tuner_c_cnr_2_qual;
+ array_size = ARRAY_SIZE(vidtv_tuner_c_cnr_2_qual);
+ break;
+
+ default:
+ dev_warn_ratelimited(dev,
+ "%s: unsupported delivery system: %u\n",
+ __func__,
+ c->delivery_system);
+ return -EINVAL;
+ }
+
+ for (i = 0; i < array_size; i++) {
+ if (cnr2qual[i].modulation != c->modulation ||
+ cnr2qual[i].fec != c->fec_inner)
+ continue;
+
+ if (!shift) {
+ *strength = cnr2qual[i].cnr_good;
+ return 0;
+ }
+ /*
+ * Channel tuned at wrong frequency. Simulate that the
+ * Carrier S/N ratio is not too good.
+ */
+
+ *strength = cnr2qual[i].cnr_ok -
+ (cnr2qual[i].cnr_good - cnr2qual[i].cnr_ok);
+ return 0;
+ }
+
+ /*
+ * do a linear interpolation between 34dB and 10dB if we can't
+ * match against the table
+ */
+ *strength = 34000 - 24000 * shift / 100;
+ return 0;
+}
+
+static int vidtv_tuner_init(struct dvb_frontend *fe)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+ struct vidtv_tuner_config config = tuner_dev->config;
+
+ msleep_interruptible(config.mock_power_up_delay_msec);
+
+ tuner_dev->hw_state.asleep = false;
+ tuner_dev->hw_state.if_frequency = 5000;
+
+ return 0;
+}
+
+static int vidtv_tuner_sleep(struct dvb_frontend *fe)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+
+ tuner_dev->hw_state.asleep = true;
+ return 0;
+}
+
+static int vidtv_tuner_suspend(struct dvb_frontend *fe)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+
+ tuner_dev->hw_state.asleep = true;
+ return 0;
+}
+
+static int vidtv_tuner_resume(struct dvb_frontend *fe)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+
+ tuner_dev->hw_state.asleep = false;
+ return 0;
+}
+
+static int vidtv_tuner_set_params(struct dvb_frontend *fe)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+ struct vidtv_tuner_config config = tuner_dev->config;
+ struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+ s32 shift;
+
+ u32 min_freq = fe->ops.tuner_ops.info.frequency_min_hz;
+ u32 max_freq = fe->ops.tuner_ops.info.frequency_max_hz;
+ u32 min_bw = fe->ops.tuner_ops.info.bandwidth_min;
+ u32 max_bw = fe->ops.tuner_ops.info.bandwidth_max;
+
+ if (c->frequency < min_freq || c->frequency > max_freq ||
+ c->bandwidth_hz < min_bw || c->bandwidth_hz > max_bw) {
+ tuner_dev->hw_state.lock_status = 0;
+ return -EINVAL;
+ }
+
+ tuner_dev->hw_state.tuned_frequency = c->frequency;
+ tuner_dev->hw_state.bandwidth = c->bandwidth_hz;
+ tuner_dev->hw_state.lock_status = TUNER_STATUS_LOCKED;
+
+ msleep_interruptible(config.mock_tune_delay_msec);
+
+ shift = vidtv_tuner_check_frequency_shift(fe);
+ if (shift < 0) {
+ tuner_dev->hw_state.lock_status = 0;
+ return shift;
+ }
+
+ return 0;
+}
+
+static int vidtv_tuner_set_config(struct dvb_frontend *fe,
+ void *priv_cfg)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+
+ memcpy(&tuner_dev->config, priv_cfg, sizeof(tuner_dev->config));
+
+ return 0;
+}
+
+static int vidtv_tuner_get_frequency(struct dvb_frontend *fe,
+ u32 *frequency)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+
+ *frequency = tuner_dev->hw_state.tuned_frequency;
+
+ return 0;
+}
+
+static int vidtv_tuner_get_bandwidth(struct dvb_frontend *fe,
+ u32 *bandwidth)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+
+ *bandwidth = tuner_dev->hw_state.bandwidth;
+
+ return 0;
+}
+
+static int vidtv_tuner_get_if_frequency(struct dvb_frontend *fe,
+ u32 *frequency)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+
+ *frequency = tuner_dev->hw_state.if_frequency;
+
+ return 0;
+}
+
+static int vidtv_tuner_get_status(struct dvb_frontend *fe, u32 *status)
+{
+ struct vidtv_tuner_dev *tuner_dev = vidtv_tuner_get_dev(fe);
+
+ *status = tuner_dev->hw_state.lock_status;
+
+ return 0;
+}
+
+static const struct dvb_tuner_ops vidtv_tuner_ops = {
+ .init = vidtv_tuner_init,
+ .sleep = vidtv_tuner_sleep,
+ .suspend = vidtv_tuner_suspend,
+ .resume = vidtv_tuner_resume,
+ .set_params = vidtv_tuner_set_params,
+ .set_config = vidtv_tuner_set_config,
+ .get_bandwidth = vidtv_tuner_get_bandwidth,
+ .get_frequency = vidtv_tuner_get_frequency,
+ .get_if_frequency = vidtv_tuner_get_if_frequency,
+ .get_status = vidtv_tuner_get_status,
+ .get_rf_strength = vidtv_tuner_get_signal_strength
+};
+
+static const struct i2c_device_id vidtv_tuner_i2c_id_table[] = {
+ {"dvb_vidtv_tuner", 0},
+ {}
+};
+MODULE_DEVICE_TABLE(i2c, vidtv_tuner_i2c_id_table);
+
+static int vidtv_tuner_i2c_probe(struct i2c_client *client,
+ const struct i2c_device_id *id)
+{
+ struct vidtv_tuner_config *config = client->dev.platform_data;
+ struct dvb_frontend *fe = config->fe;
+ struct vidtv_tuner_dev *tuner_dev = NULL;
+
+ tuner_dev = kzalloc(sizeof(*tuner_dev), GFP_KERNEL);
+ if (!tuner_dev)
+ return -ENOMEM;
+
+ tuner_dev->fe = config->fe;
+ i2c_set_clientdata(client, tuner_dev);
+
+ memcpy(&fe->ops.tuner_ops,
+ &vidtv_tuner_ops,
+ sizeof(struct dvb_tuner_ops));
+
+ memcpy(&tuner_dev->config, config, sizeof(tuner_dev->config));
+ fe->tuner_priv = client;
+
+ return 0;
+}
+
+static int vidtv_tuner_i2c_remove(struct i2c_client *client)
+{
+ struct vidtv_tuner_dev *tuner_dev = i2c_get_clientdata(client);
+
+ kfree(tuner_dev);
+
+ return 0;
+}
+
+static struct i2c_driver vidtv_tuner_i2c_driver = {
+ .driver = {
+ .name = "dvb_vidtv_tuner",
+ .suppress_bind_attrs = true,
+ },
+ .probe = vidtv_tuner_i2c_probe,
+ .remove = vidtv_tuner_i2c_remove,
+ .id_table = vidtv_tuner_i2c_id_table,
+};
+module_i2c_driver(vidtv_tuner_i2c_driver);
+
+MODULE_DESCRIPTION("Virtual DVB Tuner");
+MODULE_AUTHOR("Daniel W. S. Almeida");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/test-drivers/vidtv/vidtv_tuner.h b/drivers/media/test-drivers/vidtv/vidtv_tuner.h
new file mode 100644
index 0000000..fd55346
--- /dev/null
+++ b/drivers/media/test-drivers/vidtv/vidtv_tuner.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * The Virtual DTV test driver 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.
+ *
+ * Copyright (C) 2020 Daniel W. S. Almeida
+ */
+
+#ifndef VIDTV_TUNER_H
+#define VIDTV_TUNER_H
+
+#include <linux/types.h>
+
+#include <media/dvb_frontend.h>
+
+#define NUM_VALID_TUNER_FREQS 8
+
+/**
+ * struct vidtv_tuner_config - Configuration used to init the tuner.
+ * @fe: A pointer to the dvb_frontend structure allocated by vidtv_demod.
+ * @mock_power_up_delay_msec: Simulate a power-up delay.
+ * @mock_tune_delay_msec: Simulate a tune delay.
+ * @vidtv_valid_dvb_t_freqs: The valid DVB-T frequencies to simulate.
+ * @vidtv_valid_dvb_c_freqs: The valid DVB-C frequencies to simulate.
+ * @vidtv_valid_dvb_s_freqs: The valid DVB-S frequencies to simulate.
+ * @max_frequency_shift_hz: The maximum frequency shift in HZ allowed when
+ * tuning in a channel
+ *
+ * The configuration used to init the tuner module, usually filled
+ * by a bridge driver. For vidtv, this is filled by vidtv_bridge before the
+ * tuner module is probed.
+ */
+struct vidtv_tuner_config {
+ struct dvb_frontend *fe;
+ u32 mock_power_up_delay_msec;
+ u32 mock_tune_delay_msec;
+ u32 vidtv_valid_dvb_t_freqs[NUM_VALID_TUNER_FREQS];
+ u32 vidtv_valid_dvb_c_freqs[NUM_VALID_TUNER_FREQS];
+ u32 vidtv_valid_dvb_s_freqs[NUM_VALID_TUNER_FREQS];
+ u8 max_frequency_shift_hz;
+};
+
+#endif //VIDTV_TUNER_H
diff --git a/drivers/media/test-drivers/vim2m.c b/drivers/media/test-drivers/vim2m.c
new file mode 100644
index 0000000..a776bb8
--- /dev/null
+++ b/drivers/media/test-drivers/vim2m.c
@@ -0,0 +1,1433 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * A virtual v4l2-mem2mem example device.
+ *
+ * This is a virtual device driver for testing mem-to-mem videobuf framework.
+ * It simulates a device that uses memory buffers for both source and
+ * destination, processes the data and issues an "irq" (simulated by a delayed
+ * workqueue).
+ * The device is capable of multi-instance, multi-buffer-per-transaction
+ * operation (via the mem2mem framework).
+ *
+ * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
+ * Pawel Osciak, <pawel@osciak.com>
+ * Marek Szyprowski, <m.szyprowski@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the
+ * License, or (at your option) any later version
+ */
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/fs.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+
+#include <linux/platform_device.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-vmalloc.h>
+
+MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
+MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("0.2");
+MODULE_ALIAS("mem2mem_testdev");
+
+static unsigned int debug;
+module_param(debug, uint, 0644);
+MODULE_PARM_DESC(debug, "debug level");
+
+/* Default transaction time in msec */
+static unsigned int default_transtime = 40; /* Max 25 fps */
+module_param(default_transtime, uint, 0644);
+MODULE_PARM_DESC(default_transtime, "default transaction time in ms");
+
+#define MIN_W 32
+#define MIN_H 32
+#define MAX_W 640
+#define MAX_H 480
+
+/* Pixel alignment for non-bayer formats */
+#define WIDTH_ALIGN 2
+#define HEIGHT_ALIGN 1
+
+/* Pixel alignment for bayer formats */
+#define BAYER_WIDTH_ALIGN 2
+#define BAYER_HEIGHT_ALIGN 2
+
+/* Flags that indicate a format can be used for capture/output */
+#define MEM2MEM_CAPTURE BIT(0)
+#define MEM2MEM_OUTPUT BIT(1)
+
+#define MEM2MEM_NAME "vim2m"
+
+/* Per queue */
+#define MEM2MEM_DEF_NUM_BUFS VIDEO_MAX_FRAME
+/* In bytes, per queue */
+#define MEM2MEM_VID_MEM_LIMIT (16 * 1024 * 1024)
+
+/* Flags that indicate processing mode */
+#define MEM2MEM_HFLIP BIT(0)
+#define MEM2MEM_VFLIP BIT(1)
+
+#define dprintk(dev, lvl, fmt, arg...) \
+ v4l2_dbg(lvl, debug, &(dev)->v4l2_dev, "%s: " fmt, __func__, ## arg)
+
+static void vim2m_dev_release(struct device *dev)
+{}
+
+static struct platform_device vim2m_pdev = {
+ .name = MEM2MEM_NAME,
+ .dev.release = vim2m_dev_release,
+};
+
+struct vim2m_fmt {
+ u32 fourcc;
+ int depth;
+ /* Types the format can be used for */
+ u32 types;
+};
+
+static struct vim2m_fmt formats[] = {
+ {
+ .fourcc = V4L2_PIX_FMT_RGB565, /* rrrrrggg gggbbbbb */
+ .depth = 16,
+ .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
+ }, {
+ .fourcc = V4L2_PIX_FMT_RGB565X, /* gggbbbbb rrrrrggg */
+ .depth = 16,
+ .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
+ }, {
+ .fourcc = V4L2_PIX_FMT_RGB24,
+ .depth = 24,
+ .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
+ }, {
+ .fourcc = V4L2_PIX_FMT_BGR24,
+ .depth = 24,
+ .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
+ }, {
+ .fourcc = V4L2_PIX_FMT_YUYV,
+ .depth = 16,
+ .types = MEM2MEM_CAPTURE,
+ }, {
+ .fourcc = V4L2_PIX_FMT_SBGGR8,
+ .depth = 8,
+ .types = MEM2MEM_CAPTURE,
+ }, {
+ .fourcc = V4L2_PIX_FMT_SGBRG8,
+ .depth = 8,
+ .types = MEM2MEM_CAPTURE,
+ }, {
+ .fourcc = V4L2_PIX_FMT_SGRBG8,
+ .depth = 8,
+ .types = MEM2MEM_CAPTURE,
+ }, {
+ .fourcc = V4L2_PIX_FMT_SRGGB8,
+ .depth = 8,
+ .types = MEM2MEM_CAPTURE,
+ },
+};
+
+#define NUM_FORMATS ARRAY_SIZE(formats)
+
+/* Per-queue, driver-specific private data */
+struct vim2m_q_data {
+ unsigned int width;
+ unsigned int height;
+ unsigned int sizeimage;
+ unsigned int sequence;
+ struct vim2m_fmt *fmt;
+};
+
+enum {
+ V4L2_M2M_SRC = 0,
+ V4L2_M2M_DST = 1,
+};
+
+#define V4L2_CID_TRANS_TIME_MSEC (V4L2_CID_USER_BASE + 0x1000)
+#define V4L2_CID_TRANS_NUM_BUFS (V4L2_CID_USER_BASE + 0x1001)
+
+static struct vim2m_fmt *find_format(u32 fourcc)
+{
+ struct vim2m_fmt *fmt;
+ unsigned int k;
+
+ for (k = 0; k < NUM_FORMATS; k++) {
+ fmt = &formats[k];
+ if (fmt->fourcc == fourcc)
+ break;
+ }
+
+ if (k == NUM_FORMATS)
+ return NULL;
+
+ return &formats[k];
+}
+
+static void get_alignment(u32 fourcc,
+ unsigned int *walign, unsigned int *halign)
+{
+ switch (fourcc) {
+ case V4L2_PIX_FMT_SBGGR8:
+ case V4L2_PIX_FMT_SGBRG8:
+ case V4L2_PIX_FMT_SGRBG8:
+ case V4L2_PIX_FMT_SRGGB8:
+ *walign = BAYER_WIDTH_ALIGN;
+ *halign = BAYER_HEIGHT_ALIGN;
+ return;
+ default:
+ *walign = WIDTH_ALIGN;
+ *halign = HEIGHT_ALIGN;
+ return;
+ }
+}
+
+struct vim2m_dev {
+ struct v4l2_device v4l2_dev;
+ struct video_device vfd;
+#ifdef CONFIG_MEDIA_CONTROLLER
+ struct media_device mdev;
+#endif
+
+ atomic_t num_inst;
+ struct mutex dev_mutex;
+
+ struct v4l2_m2m_dev *m2m_dev;
+};
+
+struct vim2m_ctx {
+ struct v4l2_fh fh;
+ struct vim2m_dev *dev;
+
+ struct v4l2_ctrl_handler hdl;
+
+ /* Processed buffers in this transaction */
+ u8 num_processed;
+
+ /* Transaction length (i.e. how many buffers per transaction) */
+ u32 translen;
+ /* Transaction time (i.e. simulated processing time) in milliseconds */
+ u32 transtime;
+
+ struct mutex vb_mutex;
+ struct delayed_work work_run;
+
+ /* Abort requested by m2m */
+ int aborting;
+
+ /* Processing mode */
+ int mode;
+
+ enum v4l2_colorspace colorspace;
+ enum v4l2_ycbcr_encoding ycbcr_enc;
+ enum v4l2_xfer_func xfer_func;
+ enum v4l2_quantization quant;
+
+ /* Source and destination queue data */
+ struct vim2m_q_data q_data[2];
+};
+
+static inline struct vim2m_ctx *file2ctx(struct file *file)
+{
+ return container_of(file->private_data, struct vim2m_ctx, fh);
+}
+
+static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx,
+ enum v4l2_buf_type type)
+{
+ switch (type) {
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+ return &ctx->q_data[V4L2_M2M_SRC];
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+ return &ctx->q_data[V4L2_M2M_DST];
+ default:
+ return NULL;
+ }
+}
+
+static const char *type_name(enum v4l2_buf_type type)
+{
+ switch (type) {
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+ return "Output";
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+ return "Capture";
+ default:
+ return "Invalid";
+ }
+}
+
+#define CLIP(__color) \
+ (u8)(((__color) > 0xff) ? 0xff : (((__color) < 0) ? 0 : (__color)))
+
+static void copy_line(struct vim2m_q_data *q_data_out,
+ u8 *src, u8 *dst, bool reverse)
+{
+ int x, depth = q_data_out->fmt->depth >> 3;
+
+ if (!reverse) {
+ memcpy(dst, src, q_data_out->width * depth);
+ } else {
+ for (x = 0; x < q_data_out->width >> 1; x++) {
+ memcpy(dst, src, depth);
+ memcpy(dst + depth, src - depth, depth);
+ src -= depth << 1;
+ dst += depth << 1;
+ }
+ return;
+ }
+}
+
+static void copy_two_pixels(struct vim2m_q_data *q_data_in,
+ struct vim2m_q_data *q_data_out,
+ u8 *src[2], u8 **dst, int ypos, bool reverse)
+{
+ struct vim2m_fmt *out = q_data_out->fmt;
+ struct vim2m_fmt *in = q_data_in->fmt;
+ u8 _r[2], _g[2], _b[2], *r, *g, *b;
+ int i;
+
+ /* Step 1: read two consecutive pixels from src pointer */
+
+ r = _r;
+ g = _g;
+ b = _b;
+
+ switch (in->fourcc) {
+ case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
+ for (i = 0; i < 2; i++) {
+ u16 pix = le16_to_cpu(*(__le16 *)(src[i]));
+
+ *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;
+ *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;
+ *b++ = (u8)((pix & 0x1f) << 3) | 0x07;
+ }
+ break;
+ case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
+ for (i = 0; i < 2; i++) {
+ u16 pix = be16_to_cpu(*(__be16 *)(src[i]));
+
+ *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;
+ *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;
+ *b++ = (u8)((pix & 0x1f) << 3) | 0x07;
+ }
+ break;
+ default:
+ case V4L2_PIX_FMT_RGB24:
+ for (i = 0; i < 2; i++) {
+ *r++ = src[i][0];
+ *g++ = src[i][1];
+ *b++ = src[i][2];
+ }
+ break;
+ case V4L2_PIX_FMT_BGR24:
+ for (i = 0; i < 2; i++) {
+ *b++ = src[i][0];
+ *g++ = src[i][1];
+ *r++ = src[i][2];
+ }
+ break;
+ }
+
+ /* Step 2: store two consecutive points, reversing them if needed */
+
+ r = _r;
+ g = _g;
+ b = _b;
+
+ switch (out->fourcc) {
+ case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
+ for (i = 0; i < 2; i++) {
+ u16 pix;
+ __le16 *dst_pix = (__le16 *)*dst;
+
+ pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |
+ (*b >> 3);
+
+ *dst_pix = cpu_to_le16(pix);
+
+ *dst += 2;
+ }
+ return;
+ case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
+ for (i = 0; i < 2; i++) {
+ u16 pix;
+ __be16 *dst_pix = (__be16 *)*dst;
+
+ pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |
+ (*b >> 3);
+
+ *dst_pix = cpu_to_be16(pix);
+
+ *dst += 2;
+ }
+ return;
+ case V4L2_PIX_FMT_RGB24:
+ for (i = 0; i < 2; i++) {
+ *(*dst)++ = *r++;
+ *(*dst)++ = *g++;
+ *(*dst)++ = *b++;
+ }
+ return;
+ case V4L2_PIX_FMT_BGR24:
+ for (i = 0; i < 2; i++) {
+ *(*dst)++ = *b++;
+ *(*dst)++ = *g++;
+ *(*dst)++ = *r++;
+ }
+ return;
+ case V4L2_PIX_FMT_YUYV:
+ default:
+ {
+ u8 y, y1, u, v;
+
+ y = ((8453 * (*r) + 16594 * (*g) + 3223 * (*b)
+ + 524288) >> 15);
+ u = ((-4878 * (*r) - 9578 * (*g) + 14456 * (*b)
+ + 4210688) >> 15);
+ v = ((14456 * (*r++) - 12105 * (*g++) - 2351 * (*b++)
+ + 4210688) >> 15);
+ y1 = ((8453 * (*r) + 16594 * (*g) + 3223 * (*b)
+ + 524288) >> 15);
+
+ *(*dst)++ = y;
+ *(*dst)++ = u;
+
+ *(*dst)++ = y1;
+ *(*dst)++ = v;
+ return;
+ }
+ case V4L2_PIX_FMT_SBGGR8:
+ if (!(ypos & 1)) {
+ *(*dst)++ = *b;
+ *(*dst)++ = *++g;
+ } else {
+ *(*dst)++ = *g;
+ *(*dst)++ = *++r;
+ }
+ return;
+ case V4L2_PIX_FMT_SGBRG8:
+ if (!(ypos & 1)) {
+ *(*dst)++ = *g;
+ *(*dst)++ = *++b;
+ } else {
+ *(*dst)++ = *r;
+ *(*dst)++ = *++g;
+ }
+ return;
+ case V4L2_PIX_FMT_SGRBG8:
+ if (!(ypos & 1)) {
+ *(*dst)++ = *g;
+ *(*dst)++ = *++r;
+ } else {
+ *(*dst)++ = *b;
+ *(*dst)++ = *++g;
+ }
+ return;
+ case V4L2_PIX_FMT_SRGGB8:
+ if (!(ypos & 1)) {
+ *(*dst)++ = *r;
+ *(*dst)++ = *++g;
+ } else {
+ *(*dst)++ = *g;
+ *(*dst)++ = *++b;
+ }
+ return;
+ }
+}
+
+static int device_process(struct vim2m_ctx *ctx,
+ struct vb2_v4l2_buffer *in_vb,
+ struct vb2_v4l2_buffer *out_vb)
+{
+ struct vim2m_dev *dev = ctx->dev;
+ struct vim2m_q_data *q_data_in, *q_data_out;
+ u8 *p_in, *p_line, *p_in_x[2], *p, *p_out;
+ unsigned int width, height, bytesperline, bytes_per_pixel;
+ unsigned int x, y, y_in, y_out, x_int, x_fract, x_err, x_offset;
+ int start, end, step;
+
+ q_data_in = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+ if (!q_data_in)
+ return 0;
+ bytesperline = (q_data_in->width * q_data_in->fmt->depth) >> 3;
+ bytes_per_pixel = q_data_in->fmt->depth >> 3;
+
+ q_data_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+ if (!q_data_out)
+ return 0;
+
+ /* As we're doing scaling, use the output dimensions here */
+ height = q_data_out->height;
+ width = q_data_out->width;
+
+ p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);
+ p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);
+ if (!p_in || !p_out) {
+ v4l2_err(&dev->v4l2_dev,
+ "Acquiring kernel pointers to buffers failed\n");
+ return -EFAULT;
+ }
+
+ out_vb->sequence = q_data_out->sequence++;
+ in_vb->sequence = q_data_in->sequence++;
+ v4l2_m2m_buf_copy_metadata(in_vb, out_vb, true);
+
+ if (ctx->mode & MEM2MEM_VFLIP) {
+ start = height - 1;
+ end = -1;
+ step = -1;
+ } else {
+ start = 0;
+ end = height;
+ step = 1;
+ }
+ y_out = 0;
+
+ /*
+ * When format and resolution are identical,
+ * we can use a faster copy logic
+ */
+ if (q_data_in->fmt->fourcc == q_data_out->fmt->fourcc &&
+ q_data_in->width == q_data_out->width &&
+ q_data_in->height == q_data_out->height) {
+ for (y = start; y != end; y += step, y_out++) {
+ p = p_in + (y * bytesperline);
+ if (ctx->mode & MEM2MEM_HFLIP)
+ p += bytesperline - (q_data_in->fmt->depth >> 3);
+
+ copy_line(q_data_out, p, p_out,
+ ctx->mode & MEM2MEM_HFLIP);
+
+ p_out += bytesperline;
+ }
+ return 0;
+ }
+
+ /* Slower algorithm with format conversion, hflip, vflip and scaler */
+
+ /* To speed scaler up, use Bresenham for X dimension */
+ x_int = q_data_in->width / q_data_out->width;
+ x_fract = q_data_in->width % q_data_out->width;
+
+ for (y = start; y != end; y += step, y_out++) {
+ y_in = (y * q_data_in->height) / q_data_out->height;
+ x_offset = 0;
+ x_err = 0;
+
+ p_line = p_in + (y_in * bytesperline);
+ if (ctx->mode & MEM2MEM_HFLIP)
+ p_line += bytesperline - (q_data_in->fmt->depth >> 3);
+ p_in_x[0] = p_line;
+
+ for (x = 0; x < width >> 1; x++) {
+ x_offset += x_int;
+ x_err += x_fract;
+ if (x_err > width) {
+ x_offset++;
+ x_err -= width;
+ }
+
+ if (ctx->mode & MEM2MEM_HFLIP)
+ p_in_x[1] = p_line - x_offset * bytes_per_pixel;
+ else
+ p_in_x[1] = p_line + x_offset * bytes_per_pixel;
+
+ copy_two_pixels(q_data_in, q_data_out,
+ p_in_x, &p_out, y_out,
+ ctx->mode & MEM2MEM_HFLIP);
+
+ /* Calculate the next p_in_x0 */
+ x_offset += x_int;
+ x_err += x_fract;
+ if (x_err > width) {
+ x_offset++;
+ x_err -= width;
+ }
+
+ if (ctx->mode & MEM2MEM_HFLIP)
+ p_in_x[0] = p_line - x_offset * bytes_per_pixel;
+ else
+ p_in_x[0] = p_line + x_offset * bytes_per_pixel;
+ }
+ }
+
+ return 0;
+}
+
+/*
+ * mem2mem callbacks
+ */
+
+/*
+ * job_ready() - check whether an instance is ready to be scheduled to run
+ */
+static int job_ready(void *priv)
+{
+ struct vim2m_ctx *ctx = priv;
+
+ if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen
+ || v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen) {
+ dprintk(ctx->dev, 1, "Not enough buffers available\n");
+ return 0;
+ }
+
+ return 1;
+}
+
+static void job_abort(void *priv)
+{
+ struct vim2m_ctx *ctx = priv;
+
+ /* Will cancel the transaction in the next interrupt handler */
+ ctx->aborting = 1;
+}
+
+/* device_run() - prepares and starts the device
+ *
+ * This simulates all the immediate preparations required before starting
+ * a device. This will be called by the framework when it decides to schedule
+ * a particular instance.
+ */
+static void device_run(void *priv)
+{
+ struct vim2m_ctx *ctx = priv;
+ struct vb2_v4l2_buffer *src_buf, *dst_buf;
+
+ src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+ dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+ /* Apply request controls if any */
+ v4l2_ctrl_request_setup(src_buf->vb2_buf.req_obj.req,
+ &ctx->hdl);
+
+ device_process(ctx, src_buf, dst_buf);
+
+ /* Complete request controls if any */
+ v4l2_ctrl_request_complete(src_buf->vb2_buf.req_obj.req,
+ &ctx->hdl);
+
+ /* Run delayed work, which simulates a hardware irq */
+ schedule_delayed_work(&ctx->work_run, msecs_to_jiffies(ctx->transtime));
+}
+
+static void device_work(struct work_struct *w)
+{
+ struct vim2m_ctx *curr_ctx;
+ struct vim2m_dev *vim2m_dev;
+ struct vb2_v4l2_buffer *src_vb, *dst_vb;
+
+ curr_ctx = container_of(w, struct vim2m_ctx, work_run.work);
+
+ if (!curr_ctx) {
+ pr_err("Instance released before the end of transaction\n");
+ return;
+ }
+
+ vim2m_dev = curr_ctx->dev;
+
+ src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
+ dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
+
+ curr_ctx->num_processed++;
+
+ v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
+ v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
+
+ if (curr_ctx->num_processed == curr_ctx->translen
+ || curr_ctx->aborting) {
+ dprintk(curr_ctx->dev, 2, "Finishing capture buffer fill\n");
+ curr_ctx->num_processed = 0;
+ v4l2_m2m_job_finish(vim2m_dev->m2m_dev, curr_ctx->fh.m2m_ctx);
+ } else {
+ device_run(curr_ctx);
+ }
+}
+
+/*
+ * video ioctls
+ */
+static int vidioc_querycap(struct file *file, void *priv,
+ struct v4l2_capability *cap)
+{
+ strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
+ strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
+ snprintf(cap->bus_info, sizeof(cap->bus_info),
+ "platform:%s", MEM2MEM_NAME);
+ return 0;
+}
+
+static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
+{
+ int i, num;
+ struct vim2m_fmt *fmt;
+
+ num = 0;
+
+ for (i = 0; i < NUM_FORMATS; ++i) {
+ if (formats[i].types & type) {
+ /* index-th format of type type found ? */
+ if (num == f->index)
+ break;
+ /*
+ * Correct type but haven't reached our index yet,
+ * just increment per-type index
+ */
+ ++num;
+ }
+ }
+
+ if (i < NUM_FORMATS) {
+ /* Format found */
+ fmt = &formats[i];
+ f->pixelformat = fmt->fourcc;
+ return 0;
+ }
+
+ /* Format not found */
+ return -EINVAL;
+}
+
+static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ return enum_fmt(f, MEM2MEM_CAPTURE);
+}
+
+static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ return enum_fmt(f, MEM2MEM_OUTPUT);
+}
+
+static int vidioc_enum_framesizes(struct file *file, void *priv,
+ struct v4l2_frmsizeenum *fsize)
+{
+ if (fsize->index != 0)
+ return -EINVAL;
+
+ if (!find_format(fsize->pixel_format))
+ return -EINVAL;
+
+ fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+ fsize->stepwise.min_width = MIN_W;
+ fsize->stepwise.min_height = MIN_H;
+ fsize->stepwise.max_width = MAX_W;
+ fsize->stepwise.max_height = MAX_H;
+
+ get_alignment(fsize->pixel_format,
+ &fsize->stepwise.step_width,
+ &fsize->stepwise.step_height);
+ return 0;
+}
+
+static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
+{
+ struct vb2_queue *vq;
+ struct vim2m_q_data *q_data;
+
+ vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+ if (!vq)
+ return -EINVAL;
+
+ q_data = get_q_data(ctx, f->type);
+ if (!q_data)
+ return -EINVAL;
+
+ f->fmt.pix.width = q_data->width;
+ f->fmt.pix.height = q_data->height;
+ f->fmt.pix.field = V4L2_FIELD_NONE;
+ f->fmt.pix.pixelformat = q_data->fmt->fourcc;
+ f->fmt.pix.bytesperline = (q_data->width * q_data->fmt->depth) >> 3;
+ f->fmt.pix.sizeimage = q_data->sizeimage;
+ f->fmt.pix.colorspace = ctx->colorspace;
+ f->fmt.pix.xfer_func = ctx->xfer_func;
+ f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
+ f->fmt.pix.quantization = ctx->quant;
+
+ return 0;
+}
+
+static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ return vidioc_g_fmt(file2ctx(file), f);
+}
+
+static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ return vidioc_g_fmt(file2ctx(file), f);
+}
+
+static int vidioc_try_fmt(struct v4l2_format *f, struct vim2m_fmt *fmt)
+{
+ int walign, halign;
+ /*
+ * V4L2 specification specifies the driver corrects the
+ * format struct if any of the dimensions is unsupported
+ */
+ if (f->fmt.pix.height < MIN_H)
+ f->fmt.pix.height = MIN_H;
+ else if (f->fmt.pix.height > MAX_H)
+ f->fmt.pix.height = MAX_H;
+
+ if (f->fmt.pix.width < MIN_W)
+ f->fmt.pix.width = MIN_W;
+ else if (f->fmt.pix.width > MAX_W)
+ f->fmt.pix.width = MAX_W;
+
+ get_alignment(f->fmt.pix.pixelformat, &walign, &halign);
+ f->fmt.pix.width &= ~(walign - 1);
+ f->fmt.pix.height &= ~(halign - 1);
+ f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
+ f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
+ f->fmt.pix.field = V4L2_FIELD_NONE;
+
+ return 0;
+}
+
+static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vim2m_fmt *fmt;
+ struct vim2m_ctx *ctx = file2ctx(file);
+
+ fmt = find_format(f->fmt.pix.pixelformat);
+ if (!fmt) {
+ f->fmt.pix.pixelformat = formats[0].fourcc;
+ fmt = find_format(f->fmt.pix.pixelformat);
+ }
+ if (!(fmt->types & MEM2MEM_CAPTURE)) {
+ v4l2_err(&ctx->dev->v4l2_dev,
+ "Fourcc format (0x%08x) invalid.\n",
+ f->fmt.pix.pixelformat);
+ return -EINVAL;
+ }
+ f->fmt.pix.colorspace = ctx->colorspace;
+ f->fmt.pix.xfer_func = ctx->xfer_func;
+ f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
+ f->fmt.pix.quantization = ctx->quant;
+
+ return vidioc_try_fmt(f, fmt);
+}
+
+static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vim2m_fmt *fmt;
+ struct vim2m_ctx *ctx = file2ctx(file);
+
+ fmt = find_format(f->fmt.pix.pixelformat);
+ if (!fmt) {
+ f->fmt.pix.pixelformat = formats[0].fourcc;
+ fmt = find_format(f->fmt.pix.pixelformat);
+ }
+ if (!(fmt->types & MEM2MEM_OUTPUT)) {
+ v4l2_err(&ctx->dev->v4l2_dev,
+ "Fourcc format (0x%08x) invalid.\n",
+ f->fmt.pix.pixelformat);
+ return -EINVAL;
+ }
+ if (!f->fmt.pix.colorspace)
+ f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+
+ return vidioc_try_fmt(f, fmt);
+}
+
+static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
+{
+ struct vim2m_q_data *q_data;
+ struct vb2_queue *vq;
+
+ vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+ if (!vq)
+ return -EINVAL;
+
+ q_data = get_q_data(ctx, f->type);
+ if (!q_data)
+ return -EINVAL;
+
+ if (vb2_is_busy(vq)) {
+ v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
+ return -EBUSY;
+ }
+
+ q_data->fmt = find_format(f->fmt.pix.pixelformat);
+ q_data->width = f->fmt.pix.width;
+ q_data->height = f->fmt.pix.height;
+ q_data->sizeimage = q_data->width * q_data->height
+ * q_data->fmt->depth >> 3;
+
+ dprintk(ctx->dev, 1,
+ "Format for type %s: %dx%d (%d bpp), fmt: %c%c%c%c\n",
+ type_name(f->type), q_data->width, q_data->height,
+ q_data->fmt->depth,
+ (q_data->fmt->fourcc & 0xff),
+ (q_data->fmt->fourcc >> 8) & 0xff,
+ (q_data->fmt->fourcc >> 16) & 0xff,
+ (q_data->fmt->fourcc >> 24) & 0xff);
+
+ return 0;
+}
+
+static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ int ret;
+
+ ret = vidioc_try_fmt_vid_cap(file, priv, f);
+ if (ret)
+ return ret;
+
+ return vidioc_s_fmt(file2ctx(file), f);
+}
+
+static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vim2m_ctx *ctx = file2ctx(file);
+ int ret;
+
+ ret = vidioc_try_fmt_vid_out(file, priv, f);
+ if (ret)
+ return ret;
+
+ ret = vidioc_s_fmt(file2ctx(file), f);
+ if (!ret) {
+ ctx->colorspace = f->fmt.pix.colorspace;
+ ctx->xfer_func = f->fmt.pix.xfer_func;
+ ctx->ycbcr_enc = f->fmt.pix.ycbcr_enc;
+ ctx->quant = f->fmt.pix.quantization;
+ }
+ return ret;
+}
+
+static int vim2m_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vim2m_ctx *ctx =
+ container_of(ctrl->handler, struct vim2m_ctx, hdl);
+
+ switch (ctrl->id) {
+ case V4L2_CID_HFLIP:
+ if (ctrl->val)
+ ctx->mode |= MEM2MEM_HFLIP;
+ else
+ ctx->mode &= ~MEM2MEM_HFLIP;
+ break;
+
+ case V4L2_CID_VFLIP:
+ if (ctrl->val)
+ ctx->mode |= MEM2MEM_VFLIP;
+ else
+ ctx->mode &= ~MEM2MEM_VFLIP;
+ break;
+
+ case V4L2_CID_TRANS_TIME_MSEC:
+ ctx->transtime = ctrl->val;
+ if (ctx->transtime < 1)
+ ctx->transtime = 1;
+ break;
+
+ case V4L2_CID_TRANS_NUM_BUFS:
+ ctx->translen = ctrl->val;
+ break;
+
+ default:
+ v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vim2m_ctrl_ops = {
+ .s_ctrl = vim2m_s_ctrl,
+};
+
+static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {
+ .vidioc_querycap = vidioc_querycap,
+
+ .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
+ .vidioc_enum_framesizes = vidioc_enum_framesizes,
+ .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
+ .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
+ .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
+
+ .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
+ .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out,
+ .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
+ .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out,
+
+ .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs,
+ .vidioc_querybuf = v4l2_m2m_ioctl_querybuf,
+ .vidioc_qbuf = v4l2_m2m_ioctl_qbuf,
+ .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,
+ .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
+ .vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs,
+ .vidioc_expbuf = v4l2_m2m_ioctl_expbuf,
+
+ .vidioc_streamon = v4l2_m2m_ioctl_streamon,
+ .vidioc_streamoff = v4l2_m2m_ioctl_streamoff,
+
+ .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+ .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+/*
+ * Queue operations
+ */
+
+static int vim2m_queue_setup(struct vb2_queue *vq,
+ unsigned int *nbuffers,
+ unsigned int *nplanes,
+ unsigned int sizes[],
+ struct device *alloc_devs[])
+{
+ struct vim2m_ctx *ctx = vb2_get_drv_priv(vq);
+ struct vim2m_q_data *q_data;
+ unsigned int size, count = *nbuffers;
+
+ q_data = get_q_data(ctx, vq->type);
+ if (!q_data)
+ return -EINVAL;
+
+ size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
+
+ while (size * count > MEM2MEM_VID_MEM_LIMIT)
+ (count)--;
+ *nbuffers = count;
+
+ if (*nplanes)
+ return sizes[0] < size ? -EINVAL : 0;
+
+ *nplanes = 1;
+ sizes[0] = size;
+
+ dprintk(ctx->dev, 1, "%s: get %d buffer(s) of size %d each.\n",
+ type_name(vq->type), count, size);
+
+ return 0;
+}
+
+static int vim2m_buf_out_validate(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+ if (vbuf->field == V4L2_FIELD_ANY)
+ vbuf->field = V4L2_FIELD_NONE;
+ if (vbuf->field != V4L2_FIELD_NONE) {
+ dprintk(ctx->dev, 1, "%s field isn't supported\n", __func__);
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int vim2m_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+ struct vim2m_q_data *q_data;
+
+ dprintk(ctx->dev, 2, "type: %s\n", type_name(vb->vb2_queue->type));
+
+ q_data = get_q_data(ctx, vb->vb2_queue->type);
+ if (!q_data)
+ return -EINVAL;
+ if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
+ dprintk(ctx->dev, 1,
+ "%s data will not fit into plane (%lu < %lu)\n",
+ __func__, vb2_plane_size(vb, 0),
+ (long)q_data->sizeimage);
+ return -EINVAL;
+ }
+
+ vb2_set_plane_payload(vb, 0, q_data->sizeimage);
+
+ return 0;
+}
+
+static void vim2m_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
+}
+
+static int vim2m_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+ struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
+ struct vim2m_q_data *q_data = get_q_data(ctx, q->type);
+
+ if (!q_data)
+ return -EINVAL;
+
+ if (V4L2_TYPE_IS_OUTPUT(q->type))
+ ctx->aborting = 0;
+
+ q_data->sequence = 0;
+ return 0;
+}
+
+static void vim2m_stop_streaming(struct vb2_queue *q)
+{
+ struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
+ struct vb2_v4l2_buffer *vbuf;
+
+ cancel_delayed_work_sync(&ctx->work_run);
+
+ for (;;) {
+ if (V4L2_TYPE_IS_OUTPUT(q->type))
+ vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+ else
+ vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+ if (!vbuf)
+ return;
+ v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req,
+ &ctx->hdl);
+ v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
+ }
+}
+
+static void vim2m_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl);
+}
+
+static const struct vb2_ops vim2m_qops = {
+ .queue_setup = vim2m_queue_setup,
+ .buf_out_validate = vim2m_buf_out_validate,
+ .buf_prepare = vim2m_buf_prepare,
+ .buf_queue = vim2m_buf_queue,
+ .start_streaming = vim2m_start_streaming,
+ .stop_streaming = vim2m_stop_streaming,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+ .buf_request_complete = vim2m_buf_request_complete,
+};
+
+static int queue_init(void *priv, struct vb2_queue *src_vq,
+ struct vb2_queue *dst_vq)
+{
+ struct vim2m_ctx *ctx = priv;
+ int ret;
+
+ src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+ src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+ src_vq->drv_priv = ctx;
+ src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+ src_vq->ops = &vim2m_qops;
+ src_vq->mem_ops = &vb2_vmalloc_memops;
+ src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+ src_vq->lock = &ctx->vb_mutex;
+ src_vq->supports_requests = true;
+
+ ret = vb2_queue_init(src_vq);
+ if (ret)
+ return ret;
+
+ dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
+ dst_vq->drv_priv = ctx;
+ dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+ dst_vq->ops = &vim2m_qops;
+ dst_vq->mem_ops = &vb2_vmalloc_memops;
+ dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+ dst_vq->lock = &ctx->vb_mutex;
+
+ return vb2_queue_init(dst_vq);
+}
+
+static struct v4l2_ctrl_config vim2m_ctrl_trans_time_msec = {
+ .ops = &vim2m_ctrl_ops,
+ .id = V4L2_CID_TRANS_TIME_MSEC,
+ .name = "Transaction Time (msec)",
+ .type = V4L2_CTRL_TYPE_INTEGER,
+ .min = 1,
+ .max = 10001,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vim2m_ctrl_trans_num_bufs = {
+ .ops = &vim2m_ctrl_ops,
+ .id = V4L2_CID_TRANS_NUM_BUFS,
+ .name = "Buffers Per Transaction",
+ .type = V4L2_CTRL_TYPE_INTEGER,
+ .def = 1,
+ .min = 1,
+ .max = MEM2MEM_DEF_NUM_BUFS,
+ .step = 1,
+};
+
+/*
+ * File operations
+ */
+static int vim2m_open(struct file *file)
+{
+ struct vim2m_dev *dev = video_drvdata(file);
+ struct vim2m_ctx *ctx = NULL;
+ struct v4l2_ctrl_handler *hdl;
+ int rc = 0;
+
+ if (mutex_lock_interruptible(&dev->dev_mutex))
+ return -ERESTARTSYS;
+ ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+ if (!ctx) {
+ rc = -ENOMEM;
+ goto open_unlock;
+ }
+
+ v4l2_fh_init(&ctx->fh, video_devdata(file));
+ file->private_data = &ctx->fh;
+ ctx->dev = dev;
+ hdl = &ctx->hdl;
+ v4l2_ctrl_handler_init(hdl, 4);
+ v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
+ v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
+
+ vim2m_ctrl_trans_time_msec.def = default_transtime;
+ v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_time_msec, NULL);
+ v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_num_bufs, NULL);
+ if (hdl->error) {
+ rc = hdl->error;
+ v4l2_ctrl_handler_free(hdl);
+ kfree(ctx);
+ goto open_unlock;
+ }
+ ctx->fh.ctrl_handler = hdl;
+ v4l2_ctrl_handler_setup(hdl);
+
+ ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
+ ctx->q_data[V4L2_M2M_SRC].width = 640;
+ ctx->q_data[V4L2_M2M_SRC].height = 480;
+ ctx->q_data[V4L2_M2M_SRC].sizeimage =
+ ctx->q_data[V4L2_M2M_SRC].width *
+ ctx->q_data[V4L2_M2M_SRC].height *
+ (ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3);
+ ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
+ ctx->colorspace = V4L2_COLORSPACE_REC709;
+
+ ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
+
+ mutex_init(&ctx->vb_mutex);
+ INIT_DELAYED_WORK(&ctx->work_run, device_work);
+
+ if (IS_ERR(ctx->fh.m2m_ctx)) {
+ rc = PTR_ERR(ctx->fh.m2m_ctx);
+
+ v4l2_ctrl_handler_free(hdl);
+ v4l2_fh_exit(&ctx->fh);
+ kfree(ctx);
+ goto open_unlock;
+ }
+
+ v4l2_fh_add(&ctx->fh);
+ atomic_inc(&dev->num_inst);
+
+ dprintk(dev, 1, "Created instance: %p, m2m_ctx: %p\n",
+ ctx, ctx->fh.m2m_ctx);
+
+open_unlock:
+ mutex_unlock(&dev->dev_mutex);
+ return rc;
+}
+
+static int vim2m_release(struct file *file)
+{
+ struct vim2m_dev *dev = video_drvdata(file);
+ struct vim2m_ctx *ctx = file2ctx(file);
+
+ dprintk(dev, 1, "Releasing instance %p\n", ctx);
+
+ v4l2_fh_del(&ctx->fh);
+ v4l2_fh_exit(&ctx->fh);
+ v4l2_ctrl_handler_free(&ctx->hdl);
+ mutex_lock(&dev->dev_mutex);
+ v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+ mutex_unlock(&dev->dev_mutex);
+ kfree(ctx);
+
+ atomic_dec(&dev->num_inst);
+
+ return 0;
+}
+
+static void vim2m_device_release(struct video_device *vdev)
+{
+ struct vim2m_dev *dev = container_of(vdev, struct vim2m_dev, vfd);
+
+ v4l2_device_unregister(&dev->v4l2_dev);
+ v4l2_m2m_release(dev->m2m_dev);
+#ifdef CONFIG_MEDIA_CONTROLLER
+ media_device_cleanup(&dev->mdev);
+#endif
+ kfree(dev);
+}
+
+static const struct v4l2_file_operations vim2m_fops = {
+ .owner = THIS_MODULE,
+ .open = vim2m_open,
+ .release = vim2m_release,
+ .poll = v4l2_m2m_fop_poll,
+ .unlocked_ioctl = video_ioctl2,
+ .mmap = v4l2_m2m_fop_mmap,
+};
+
+static const struct video_device vim2m_videodev = {
+ .name = MEM2MEM_NAME,
+ .vfl_dir = VFL_DIR_M2M,
+ .fops = &vim2m_fops,
+ .ioctl_ops = &vim2m_ioctl_ops,
+ .minor = -1,
+ .release = vim2m_device_release,
+ .device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
+};
+
+static const struct v4l2_m2m_ops m2m_ops = {
+ .device_run = device_run,
+ .job_ready = job_ready,
+ .job_abort = job_abort,
+};
+
+static const struct media_device_ops m2m_media_ops = {
+ .req_validate = vb2_request_validate,
+ .req_queue = v4l2_m2m_request_queue,
+};
+
+static int vim2m_probe(struct platform_device *pdev)
+{
+ struct vim2m_dev *dev;
+ struct video_device *vfd;
+ int ret;
+
+ dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+ if (!dev)
+ return -ENOMEM;
+
+ ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+ if (ret)
+ goto error_free;
+
+ atomic_set(&dev->num_inst, 0);
+ mutex_init(&dev->dev_mutex);
+
+ dev->vfd = vim2m_videodev;
+ vfd = &dev->vfd;
+ vfd->lock = &dev->dev_mutex;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+
+ ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
+ if (ret) {
+ v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
+ goto error_v4l2;
+ }
+
+ video_set_drvdata(vfd, dev);
+ v4l2_info(&dev->v4l2_dev,
+ "Device registered as /dev/video%d\n", vfd->num);
+
+ platform_set_drvdata(pdev, dev);
+
+ dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
+ if (IS_ERR(dev->m2m_dev)) {
+ v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
+ ret = PTR_ERR(dev->m2m_dev);
+ dev->m2m_dev = NULL;
+ goto error_dev;
+ }
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->mdev.dev = &pdev->dev;
+ strscpy(dev->mdev.model, "vim2m", sizeof(dev->mdev.model));
+ strscpy(dev->mdev.bus_info, "platform:vim2m",
+ sizeof(dev->mdev.bus_info));
+ media_device_init(&dev->mdev);
+ dev->mdev.ops = &m2m_media_ops;
+ dev->v4l2_dev.mdev = &dev->mdev;
+
+ ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd,
+ MEDIA_ENT_F_PROC_VIDEO_SCALER);
+ if (ret) {
+ v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n");
+ goto error_dev;
+ }
+
+ ret = media_device_register(&dev->mdev);
+ if (ret) {
+ v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n");
+ goto error_m2m_mc;
+ }
+#endif
+ return 0;
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+error_m2m_mc:
+ v4l2_m2m_unregister_media_controller(dev->m2m_dev);
+#endif
+error_dev:
+ video_unregister_device(&dev->vfd);
+ /* vim2m_device_release called by video_unregister_device to release various objects */
+ return ret;
+error_v4l2:
+ v4l2_device_unregister(&dev->v4l2_dev);
+error_free:
+ kfree(dev);
+
+ return ret;
+}
+
+static int vim2m_remove(struct platform_device *pdev)
+{
+ struct vim2m_dev *dev = platform_get_drvdata(pdev);
+
+ v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME);
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ media_device_unregister(&dev->mdev);
+ v4l2_m2m_unregister_media_controller(dev->m2m_dev);
+#endif
+ video_unregister_device(&dev->vfd);
+
+ return 0;
+}
+
+static struct platform_driver vim2m_pdrv = {
+ .probe = vim2m_probe,
+ .remove = vim2m_remove,
+ .driver = {
+ .name = MEM2MEM_NAME,
+ },
+};
+
+static void __exit vim2m_exit(void)
+{
+ platform_driver_unregister(&vim2m_pdrv);
+ platform_device_unregister(&vim2m_pdev);
+}
+
+static int __init vim2m_init(void)
+{
+ int ret;
+
+ ret = platform_device_register(&vim2m_pdev);
+ if (ret)
+ return ret;
+
+ ret = platform_driver_register(&vim2m_pdrv);
+ if (ret)
+ platform_device_unregister(&vim2m_pdev);
+
+ return ret;
+}
+
+module_init(vim2m_init);
+module_exit(vim2m_exit);
diff --git a/drivers/media/test-drivers/vimc/Kconfig b/drivers/media/test-drivers/vimc/Kconfig
new file mode 100644
index 0000000..da4b2ad
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/Kconfig
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_VIMC
+ tristate "Virtual Media Controller Driver (VIMC)"
+ depends on VIDEO_DEV && VIDEO_V4L2
+ select FONT_SUPPORT
+ select FONT_8x16
+ select MEDIA_CONTROLLER
+ select VIDEO_V4L2_SUBDEV_API
+ select VIDEOBUF2_VMALLOC
+ select VIDEO_V4L2_TPG
+ help
+ Skeleton driver for Virtual Media Controller
+
+ This driver can be compared to the vivid driver for emulating
+ a media node that exposes a complex media topology. The topology
+ is hard coded for now but is meant to be highly configurable in
+ the future.
+
+ When in doubt, say N.
diff --git a/drivers/media/test-drivers/vimc/Makefile b/drivers/media/test-drivers/vimc/Makefile
new file mode 100644
index 0000000..a53b2b5
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/Makefile
@@ -0,0 +1,6 @@
+# SPDX-License-Identifier: GPL-2.0
+vimc-y := vimc-core.o vimc-common.o vimc-streamer.o vimc-capture.o \
+ vimc-debayer.o vimc-scaler.o vimc-sensor.o
+
+obj-$(CONFIG_VIDEO_VIMC) += vimc.o
+
diff --git a/drivers/media/test-drivers/vimc/vimc-capture.c b/drivers/media/test-drivers/vimc/vimc-capture.c
new file mode 100644
index 0000000..5e9fd90
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/vimc-capture.c
@@ -0,0 +1,496 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-capture.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <media/v4l2-ioctl.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "vimc-common.h"
+#include "vimc-streamer.h"
+
+struct vimc_cap_device {
+ struct vimc_ent_device ved;
+ struct video_device vdev;
+ struct v4l2_pix_format format;
+ struct vb2_queue queue;
+ struct list_head buf_list;
+ /*
+ * NOTE: in a real driver, a spin lock must be used to access the
+ * queue because the frames are generated from a hardware interruption
+ * and the isr is not allowed to sleep.
+ * Even if it is not necessary a spinlock in the vimc driver, we
+ * use it here as a code reference
+ */
+ spinlock_t qlock;
+ struct mutex lock;
+ u32 sequence;
+ struct vimc_stream stream;
+ struct media_pad pad;
+};
+
+static const struct v4l2_pix_format fmt_default = {
+ .width = 640,
+ .height = 480,
+ .pixelformat = V4L2_PIX_FMT_RGB24,
+ .field = V4L2_FIELD_NONE,
+ .colorspace = V4L2_COLORSPACE_SRGB,
+};
+
+struct vimc_cap_buffer {
+ /*
+ * struct vb2_v4l2_buffer must be the first element
+ * the videobuf2 framework will allocate this struct based on
+ * buf_struct_size and use the first sizeof(struct vb2_buffer) bytes of
+ * memory as a vb2_buffer
+ */
+ struct vb2_v4l2_buffer vb2;
+ struct list_head list;
+};
+
+static int vimc_cap_querycap(struct file *file, void *priv,
+ struct v4l2_capability *cap)
+{
+ strscpy(cap->driver, VIMC_PDEV_NAME, sizeof(cap->driver));
+ strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
+ snprintf(cap->bus_info, sizeof(cap->bus_info),
+ "platform:%s", VIMC_PDEV_NAME);
+
+ return 0;
+}
+
+static void vimc_cap_get_format(struct vimc_ent_device *ved,
+ struct v4l2_pix_format *fmt)
+{
+ struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
+ ved);
+
+ *fmt = vcap->format;
+}
+
+static int vimc_cap_g_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vimc_cap_device *vcap = video_drvdata(file);
+
+ f->fmt.pix = vcap->format;
+
+ return 0;
+}
+
+static int vimc_cap_try_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct v4l2_pix_format *format = &f->fmt.pix;
+ const struct vimc_pix_map *vpix;
+
+ format->width = clamp_t(u32, format->width, VIMC_FRAME_MIN_WIDTH,
+ VIMC_FRAME_MAX_WIDTH) & ~1;
+ format->height = clamp_t(u32, format->height, VIMC_FRAME_MIN_HEIGHT,
+ VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+ /* Don't accept a pixelformat that is not on the table */
+ vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
+ if (!vpix) {
+ format->pixelformat = fmt_default.pixelformat;
+ vpix = vimc_pix_map_by_pixelformat(format->pixelformat);
+ }
+ /* TODO: Add support for custom bytesperline values */
+ format->bytesperline = format->width * vpix->bpp;
+ format->sizeimage = format->bytesperline * format->height;
+
+ if (format->field == V4L2_FIELD_ANY)
+ format->field = fmt_default.field;
+
+ vimc_colorimetry_clamp(format);
+
+ if (format->colorspace == V4L2_COLORSPACE_DEFAULT)
+ format->colorspace = fmt_default.colorspace;
+
+ return 0;
+}
+
+static int vimc_cap_s_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vimc_cap_device *vcap = video_drvdata(file);
+ int ret;
+
+ /* Do not change the format while stream is on */
+ if (vb2_is_busy(&vcap->queue))
+ return -EBUSY;
+
+ ret = vimc_cap_try_fmt_vid_cap(file, priv, f);
+ if (ret)
+ return ret;
+
+ dev_dbg(vcap->ved.dev, "%s: format update: "
+ "old:%dx%d (0x%x, %d, %d, %d, %d) "
+ "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vcap->vdev.name,
+ /* old */
+ vcap->format.width, vcap->format.height,
+ vcap->format.pixelformat, vcap->format.colorspace,
+ vcap->format.quantization, vcap->format.xfer_func,
+ vcap->format.ycbcr_enc,
+ /* new */
+ f->fmt.pix.width, f->fmt.pix.height,
+ f->fmt.pix.pixelformat, f->fmt.pix.colorspace,
+ f->fmt.pix.quantization, f->fmt.pix.xfer_func,
+ f->fmt.pix.ycbcr_enc);
+
+ vcap->format = f->fmt.pix;
+
+ return 0;
+}
+
+static int vimc_cap_enum_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ const struct vimc_pix_map *vpix;
+
+ if (f->mbus_code) {
+ if (f->index > 0)
+ return -EINVAL;
+
+ vpix = vimc_pix_map_by_code(f->mbus_code);
+ } else {
+ vpix = vimc_pix_map_by_index(f->index);
+ }
+
+ if (!vpix)
+ return -EINVAL;
+
+ f->pixelformat = vpix->pixelformat;
+
+ return 0;
+}
+
+static int vimc_cap_enum_framesizes(struct file *file, void *fh,
+ struct v4l2_frmsizeenum *fsize)
+{
+ const struct vimc_pix_map *vpix;
+
+ if (fsize->index)
+ return -EINVAL;
+
+ /* Only accept code in the pix map table */
+ vpix = vimc_pix_map_by_code(fsize->pixel_format);
+ if (!vpix)
+ return -EINVAL;
+
+ fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
+ fsize->stepwise.min_width = VIMC_FRAME_MIN_WIDTH;
+ fsize->stepwise.max_width = VIMC_FRAME_MAX_WIDTH;
+ fsize->stepwise.min_height = VIMC_FRAME_MIN_HEIGHT;
+ fsize->stepwise.max_height = VIMC_FRAME_MAX_HEIGHT;
+ fsize->stepwise.step_width = 1;
+ fsize->stepwise.step_height = 1;
+
+ return 0;
+}
+
+static const struct v4l2_file_operations vimc_cap_fops = {
+ .owner = THIS_MODULE,
+ .open = v4l2_fh_open,
+ .release = vb2_fop_release,
+ .read = vb2_fop_read,
+ .poll = vb2_fop_poll,
+ .unlocked_ioctl = video_ioctl2,
+ .mmap = vb2_fop_mmap,
+};
+
+static const struct v4l2_ioctl_ops vimc_cap_ioctl_ops = {
+ .vidioc_querycap = vimc_cap_querycap,
+
+ .vidioc_g_fmt_vid_cap = vimc_cap_g_fmt_vid_cap,
+ .vidioc_s_fmt_vid_cap = vimc_cap_s_fmt_vid_cap,
+ .vidioc_try_fmt_vid_cap = vimc_cap_try_fmt_vid_cap,
+ .vidioc_enum_fmt_vid_cap = vimc_cap_enum_fmt_vid_cap,
+ .vidioc_enum_framesizes = vimc_cap_enum_framesizes,
+
+ .vidioc_reqbufs = vb2_ioctl_reqbufs,
+ .vidioc_create_bufs = vb2_ioctl_create_bufs,
+ .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+ .vidioc_querybuf = vb2_ioctl_querybuf,
+ .vidioc_qbuf = vb2_ioctl_qbuf,
+ .vidioc_dqbuf = vb2_ioctl_dqbuf,
+ .vidioc_expbuf = vb2_ioctl_expbuf,
+ .vidioc_streamon = vb2_ioctl_streamon,
+ .vidioc_streamoff = vb2_ioctl_streamoff,
+};
+
+static void vimc_cap_return_all_buffers(struct vimc_cap_device *vcap,
+ enum vb2_buffer_state state)
+{
+ struct vimc_cap_buffer *vbuf, *node;
+
+ spin_lock(&vcap->qlock);
+
+ list_for_each_entry_safe(vbuf, node, &vcap->buf_list, list) {
+ list_del(&vbuf->list);
+ vb2_buffer_done(&vbuf->vb2.vb2_buf, state);
+ }
+
+ spin_unlock(&vcap->qlock);
+}
+
+static int vimc_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+ struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
+ struct media_entity *entity = &vcap->vdev.entity;
+ int ret;
+
+ vcap->sequence = 0;
+
+ /* Start the media pipeline */
+ ret = media_pipeline_start(entity, &vcap->stream.pipe);
+ if (ret) {
+ vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
+ return ret;
+ }
+
+ ret = vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 1);
+ if (ret) {
+ media_pipeline_stop(entity);
+ vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_QUEUED);
+ return ret;
+ }
+
+ return 0;
+}
+
+/*
+ * Stop the stream engine. Any remaining buffers in the stream queue are
+ * dequeued and passed on to the vb2 framework marked as STATE_ERROR.
+ */
+static void vimc_cap_stop_streaming(struct vb2_queue *vq)
+{
+ struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
+
+ vimc_streamer_s_stream(&vcap->stream, &vcap->ved, 0);
+
+ /* Stop the media pipeline */
+ media_pipeline_stop(&vcap->vdev.entity);
+
+ /* Release all active buffers */
+ vimc_cap_return_all_buffers(vcap, VB2_BUF_STATE_ERROR);
+}
+
+static void vimc_cap_buf_queue(struct vb2_buffer *vb2_buf)
+{
+ struct vimc_cap_device *vcap = vb2_get_drv_priv(vb2_buf->vb2_queue);
+ struct vimc_cap_buffer *buf = container_of(vb2_buf,
+ struct vimc_cap_buffer,
+ vb2.vb2_buf);
+
+ spin_lock(&vcap->qlock);
+ list_add_tail(&buf->list, &vcap->buf_list);
+ spin_unlock(&vcap->qlock);
+}
+
+static int vimc_cap_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+ unsigned int *nplanes, unsigned int sizes[],
+ struct device *alloc_devs[])
+{
+ struct vimc_cap_device *vcap = vb2_get_drv_priv(vq);
+
+ if (*nplanes)
+ return sizes[0] < vcap->format.sizeimage ? -EINVAL : 0;
+ /* We don't support multiplanes for now */
+ *nplanes = 1;
+ sizes[0] = vcap->format.sizeimage;
+
+ return 0;
+}
+
+static int vimc_cap_buffer_prepare(struct vb2_buffer *vb)
+{
+ struct vimc_cap_device *vcap = vb2_get_drv_priv(vb->vb2_queue);
+ unsigned long size = vcap->format.sizeimage;
+
+ if (vb2_plane_size(vb, 0) < size) {
+ dev_err(vcap->ved.dev, "%s: buffer too small (%lu < %lu)\n",
+ vcap->vdev.name, vb2_plane_size(vb, 0), size);
+ return -EINVAL;
+ }
+ return 0;
+}
+
+static const struct vb2_ops vimc_cap_qops = {
+ .start_streaming = vimc_cap_start_streaming,
+ .stop_streaming = vimc_cap_stop_streaming,
+ .buf_queue = vimc_cap_buf_queue,
+ .queue_setup = vimc_cap_queue_setup,
+ .buf_prepare = vimc_cap_buffer_prepare,
+ /*
+ * Since q->lock is set we can use the standard
+ * vb2_ops_wait_prepare/finish helper functions.
+ */
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+static const struct media_entity_operations vimc_cap_mops = {
+ .link_validate = vimc_vdev_link_validate,
+};
+
+static void vimc_cap_release(struct vimc_ent_device *ved)
+{
+ struct vimc_cap_device *vcap =
+ container_of(ved, struct vimc_cap_device, ved);
+
+ media_entity_cleanup(vcap->ved.ent);
+ kfree(vcap);
+}
+
+static void vimc_cap_unregister(struct vimc_ent_device *ved)
+{
+ struct vimc_cap_device *vcap =
+ container_of(ved, struct vimc_cap_device, ved);
+
+ vb2_video_unregister_device(&vcap->vdev);
+}
+
+static void *vimc_cap_process_frame(struct vimc_ent_device *ved,
+ const void *frame)
+{
+ struct vimc_cap_device *vcap = container_of(ved, struct vimc_cap_device,
+ ved);
+ struct vimc_cap_buffer *vimc_buf;
+ void *vbuf;
+
+ spin_lock(&vcap->qlock);
+
+ /* Get the first entry of the list */
+ vimc_buf = list_first_entry_or_null(&vcap->buf_list,
+ typeof(*vimc_buf), list);
+ if (!vimc_buf) {
+ spin_unlock(&vcap->qlock);
+ return ERR_PTR(-EAGAIN);
+ }
+
+ /* Remove this entry from the list */
+ list_del(&vimc_buf->list);
+
+ spin_unlock(&vcap->qlock);
+
+ /* Fill the buffer */
+ vimc_buf->vb2.vb2_buf.timestamp = ktime_get_ns();
+ vimc_buf->vb2.sequence = vcap->sequence++;
+ vimc_buf->vb2.field = vcap->format.field;
+
+ vbuf = vb2_plane_vaddr(&vimc_buf->vb2.vb2_buf, 0);
+
+ memcpy(vbuf, frame, vcap->format.sizeimage);
+
+ /* Set it as ready */
+ vb2_set_plane_payload(&vimc_buf->vb2.vb2_buf, 0,
+ vcap->format.sizeimage);
+ vb2_buffer_done(&vimc_buf->vb2.vb2_buf, VB2_BUF_STATE_DONE);
+ return NULL;
+}
+
+static struct vimc_ent_device *vimc_cap_add(struct vimc_device *vimc,
+ const char *vcfg_name)
+{
+ struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
+ const struct vimc_pix_map *vpix;
+ struct vimc_cap_device *vcap;
+ struct video_device *vdev;
+ struct vb2_queue *q;
+ int ret;
+
+ /* Allocate the vimc_cap_device struct */
+ vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
+ if (!vcap)
+ return ERR_PTR(-ENOMEM);
+
+ /* Initialize the media entity */
+ vcap->vdev.entity.name = vcfg_name;
+ vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
+ vcap->pad.flags = MEDIA_PAD_FL_SINK;
+ ret = media_entity_pads_init(&vcap->vdev.entity,
+ 1, &vcap->pad);
+ if (ret)
+ goto err_free_vcap;
+
+ /* Initialize the lock */
+ mutex_init(&vcap->lock);
+
+ /* Initialize the vb2 queue */
+ q = &vcap->queue;
+ q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ q->io_modes = VB2_MMAP | VB2_DMABUF | VB2_USERPTR;
+ q->drv_priv = vcap;
+ q->buf_struct_size = sizeof(struct vimc_cap_buffer);
+ q->ops = &vimc_cap_qops;
+ q->mem_ops = &vb2_vmalloc_memops;
+ q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+ q->min_buffers_needed = 2;
+ q->lock = &vcap->lock;
+
+ ret = vb2_queue_init(q);
+ if (ret) {
+ dev_err(vimc->mdev.dev, "%s: vb2 queue init failed (err=%d)\n",
+ vcfg_name, ret);
+ goto err_clean_m_ent;
+ }
+
+ /* Initialize buffer list and its lock */
+ INIT_LIST_HEAD(&vcap->buf_list);
+ spin_lock_init(&vcap->qlock);
+
+ /* Set default frame format */
+ vcap->format = fmt_default;
+ vpix = vimc_pix_map_by_pixelformat(vcap->format.pixelformat);
+ vcap->format.bytesperline = vcap->format.width * vpix->bpp;
+ vcap->format.sizeimage = vcap->format.bytesperline *
+ vcap->format.height;
+
+ /* Fill the vimc_ent_device struct */
+ vcap->ved.ent = &vcap->vdev.entity;
+ vcap->ved.process_frame = vimc_cap_process_frame;
+ vcap->ved.vdev_get_format = vimc_cap_get_format;
+ vcap->ved.dev = vimc->mdev.dev;
+
+ /* Initialize the video_device struct */
+ vdev = &vcap->vdev;
+ vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
+ | V4L2_CAP_IO_MC;
+ vdev->entity.ops = &vimc_cap_mops;
+ vdev->release = video_device_release_empty;
+ vdev->fops = &vimc_cap_fops;
+ vdev->ioctl_ops = &vimc_cap_ioctl_ops;
+ vdev->lock = &vcap->lock;
+ vdev->queue = q;
+ vdev->v4l2_dev = v4l2_dev;
+ vdev->vfl_dir = VFL_DIR_RX;
+ strscpy(vdev->name, vcfg_name, sizeof(vdev->name));
+ video_set_drvdata(vdev, &vcap->ved);
+
+ /* Register the video_device with the v4l2 and the media framework */
+ ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
+ if (ret) {
+ dev_err(vimc->mdev.dev, "%s: video register failed (err=%d)\n",
+ vcap->vdev.name, ret);
+ goto err_clean_m_ent;
+ }
+
+ return &vcap->ved;
+
+err_clean_m_ent:
+ media_entity_cleanup(&vcap->vdev.entity);
+err_free_vcap:
+ kfree(vcap);
+
+ return ERR_PTR(ret);
+}
+
+struct vimc_ent_type vimc_cap_type = {
+ .add = vimc_cap_add,
+ .unregister = vimc_cap_unregister,
+ .release = vimc_cap_release
+};
diff --git a/drivers/media/test-drivers/vimc/vimc-common.c b/drivers/media/test-drivers/vimc/vimc-common.c
new file mode 100644
index 0000000..7b27153
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/vimc-common.c
@@ -0,0 +1,400 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-common.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+
+#include "vimc-common.h"
+
+/*
+ * NOTE: non-bayer formats need to come first (necessary for enum_mbus_code
+ * in the scaler)
+ */
+static const struct vimc_pix_map vimc_pix_map_list[] = {
+ /* TODO: add all missing formats */
+
+ /* RGB formats */
+ {
+ .code = {
+ MEDIA_BUS_FMT_BGR888_1X24,
+ MEDIA_BUS_FMT_BGR888_3X8
+ },
+ .pixelformat = V4L2_PIX_FMT_BGR24,
+ .bpp = 3,
+ .bayer = false,
+ },
+ {
+ .code = {
+ MEDIA_BUS_FMT_RGB888_1X24,
+ MEDIA_BUS_FMT_RGB888_2X12_BE,
+ MEDIA_BUS_FMT_RGB888_2X12_LE,
+ MEDIA_BUS_FMT_RGB888_3X8,
+ MEDIA_BUS_FMT_RGB888_1X7X4_SPWG,
+ MEDIA_BUS_FMT_RGB888_1X7X4_JEIDA,
+ MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+ MEDIA_BUS_FMT_GBR888_1X24
+ },
+ .pixelformat = V4L2_PIX_FMT_RGB24,
+ .bpp = 3,
+ .bayer = false,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_ARGB8888_1X32 },
+ .pixelformat = V4L2_PIX_FMT_ARGB32,
+ .bpp = 4,
+ .bayer = false,
+ },
+
+ /* Bayer formats */
+ {
+ .code = { MEDIA_BUS_FMT_SBGGR8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SBGGR8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGBRG8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SGBRG8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGRBG8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SGRBG8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SRGGB8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SRGGB8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SBGGR10_1X10 },
+ .pixelformat = V4L2_PIX_FMT_SBGGR10,
+ .bpp = 2,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGBRG10_1X10 },
+ .pixelformat = V4L2_PIX_FMT_SGBRG10,
+ .bpp = 2,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGRBG10_1X10 },
+ .pixelformat = V4L2_PIX_FMT_SGRBG10,
+ .bpp = 2,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SRGGB10_1X10 },
+ .pixelformat = V4L2_PIX_FMT_SRGGB10,
+ .bpp = 2,
+ .bayer = true,
+ },
+
+ /* 10bit raw bayer a-law compressed to 8 bits */
+ {
+ .code = { MEDIA_BUS_FMT_SBGGR10_ALAW8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SBGGR10ALAW8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGBRG10_ALAW8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SGBRG10ALAW8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGRBG10_ALAW8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SGRBG10ALAW8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SRGGB10_ALAW8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SRGGB10ALAW8,
+ .bpp = 1,
+ .bayer = true,
+ },
+
+ /* 10bit raw bayer DPCM compressed to 8 bits */
+ {
+ .code = { MEDIA_BUS_FMT_SBGGR10_DPCM8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SBGGR10DPCM8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGBRG10_DPCM8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SGBRG10DPCM8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGRBG10_DPCM8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SGRBG10DPCM8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SRGGB10_DPCM8_1X8 },
+ .pixelformat = V4L2_PIX_FMT_SRGGB10DPCM8,
+ .bpp = 1,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SBGGR12_1X12 },
+ .pixelformat = V4L2_PIX_FMT_SBGGR12,
+ .bpp = 2,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGBRG12_1X12 },
+ .pixelformat = V4L2_PIX_FMT_SGBRG12,
+ .bpp = 2,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SGRBG12_1X12 },
+ .pixelformat = V4L2_PIX_FMT_SGRBG12,
+ .bpp = 2,
+ .bayer = true,
+ },
+ {
+ .code = { MEDIA_BUS_FMT_SRGGB12_1X12 },
+ .pixelformat = V4L2_PIX_FMT_SRGGB12,
+ .bpp = 2,
+ .bayer = true,
+ },
+};
+
+bool vimc_is_source(struct media_entity *ent)
+{
+ unsigned int i;
+
+ for (i = 0; i < ent->num_pads; i++)
+ if (ent->pads[i].flags & MEDIA_PAD_FL_SINK)
+ return false;
+ return true;
+}
+
+const struct vimc_pix_map *vimc_pix_map_by_index(unsigned int i)
+{
+ if (i >= ARRAY_SIZE(vimc_pix_map_list))
+ return NULL;
+
+ return &vimc_pix_map_list[i];
+}
+
+u32 vimc_mbus_code_by_index(unsigned int index)
+{
+ unsigned int i, j;
+
+ for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+ for (j = 0; j < ARRAY_SIZE(vimc_pix_map_list[i].code); j++) {
+ if (!vimc_pix_map_list[i].code[j])
+ break;
+
+ if (!index)
+ return vimc_pix_map_list[i].code[j];
+ index--;
+ }
+ }
+ return 0;
+}
+
+const struct vimc_pix_map *vimc_pix_map_by_code(u32 code)
+{
+ unsigned int i, j;
+
+ for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+ for (j = 0; j < ARRAY_SIZE(vimc_pix_map_list[i].code); j++) {
+ if (vimc_pix_map_list[i].code[j] == code)
+ return &vimc_pix_map_list[i];
+ }
+ }
+ return NULL;
+}
+
+const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat)
+{
+ unsigned int i;
+
+ for (i = 0; i < ARRAY_SIZE(vimc_pix_map_list); i++) {
+ if (vimc_pix_map_list[i].pixelformat == pixelformat)
+ return &vimc_pix_map_list[i];
+ }
+ return NULL;
+}
+
+static int vimc_get_pix_format(struct media_pad *pad,
+ struct v4l2_pix_format *fmt)
+{
+ if (is_media_entity_v4l2_subdev(pad->entity)) {
+ struct v4l2_subdev *sd =
+ media_entity_to_v4l2_subdev(pad->entity);
+ struct v4l2_subdev_format sd_fmt;
+ const struct vimc_pix_map *pix_map;
+ int ret;
+
+ sd_fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
+ sd_fmt.pad = pad->index;
+
+ ret = v4l2_subdev_call(sd, pad, get_fmt, NULL, &sd_fmt);
+ if (ret)
+ return ret;
+
+ v4l2_fill_pix_format(fmt, &sd_fmt.format);
+ pix_map = vimc_pix_map_by_code(sd_fmt.format.code);
+ fmt->pixelformat = pix_map->pixelformat;
+ } else if (is_media_entity_v4l2_video_device(pad->entity)) {
+ struct video_device *vdev = container_of(pad->entity,
+ struct video_device,
+ entity);
+ struct vimc_ent_device *ved = video_get_drvdata(vdev);
+
+ if (!ved->vdev_get_format)
+ return -ENOIOCTLCMD;
+
+ ved->vdev_get_format(ved, fmt);
+ } else {
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+int vimc_vdev_link_validate(struct media_link *link)
+{
+ struct v4l2_pix_format source_fmt, sink_fmt;
+ int ret;
+
+ ret = vimc_get_pix_format(link->source, &source_fmt);
+ if (ret)
+ return ret;
+
+ ret = vimc_get_pix_format(link->sink, &sink_fmt);
+ if (ret)
+ return ret;
+
+ pr_info("vimc link validate: "
+ "%s:src:%dx%d (0x%x, %d, %d, %d, %d) "
+ "%s:snk:%dx%d (0x%x, %d, %d, %d, %d)\n",
+ /* src */
+ link->source->entity->name,
+ source_fmt.width, source_fmt.height,
+ source_fmt.pixelformat, source_fmt.colorspace,
+ source_fmt.quantization, source_fmt.xfer_func,
+ source_fmt.ycbcr_enc,
+ /* sink */
+ link->sink->entity->name,
+ sink_fmt.width, sink_fmt.height,
+ sink_fmt.pixelformat, sink_fmt.colorspace,
+ sink_fmt.quantization, sink_fmt.xfer_func,
+ sink_fmt.ycbcr_enc);
+
+ /* The width, height and pixelformat must match. */
+ if (source_fmt.width != sink_fmt.width ||
+ source_fmt.height != sink_fmt.height ||
+ source_fmt.pixelformat != sink_fmt.pixelformat)
+ return -EPIPE;
+
+ /*
+ * The field order must match, or the sink field order must be NONE
+ * to support interlaced hardware connected to bridges that support
+ * progressive formats only.
+ */
+ if (source_fmt.field != sink_fmt.field &&
+ sink_fmt.field != V4L2_FIELD_NONE)
+ return -EPIPE;
+
+ /*
+ * If colorspace is DEFAULT, then assume all the colorimetry is also
+ * DEFAULT, return 0 to skip comparing the other colorimetry parameters
+ */
+ if (source_fmt.colorspace == V4L2_COLORSPACE_DEFAULT ||
+ sink_fmt.colorspace == V4L2_COLORSPACE_DEFAULT)
+ return 0;
+
+ /* Colorspace must match. */
+ if (source_fmt.colorspace != sink_fmt.colorspace)
+ return -EPIPE;
+
+ /* Colorimetry must match if they are not set to DEFAULT */
+ if (source_fmt.ycbcr_enc != V4L2_YCBCR_ENC_DEFAULT &&
+ sink_fmt.ycbcr_enc != V4L2_YCBCR_ENC_DEFAULT &&
+ source_fmt.ycbcr_enc != sink_fmt.ycbcr_enc)
+ return -EPIPE;
+
+ if (source_fmt.quantization != V4L2_QUANTIZATION_DEFAULT &&
+ sink_fmt.quantization != V4L2_QUANTIZATION_DEFAULT &&
+ source_fmt.quantization != sink_fmt.quantization)
+ return -EPIPE;
+
+ if (source_fmt.xfer_func != V4L2_XFER_FUNC_DEFAULT &&
+ sink_fmt.xfer_func != V4L2_XFER_FUNC_DEFAULT &&
+ source_fmt.xfer_func != sink_fmt.xfer_func)
+ return -EPIPE;
+
+ return 0;
+}
+
+static const struct media_entity_operations vimc_ent_sd_mops = {
+ .link_validate = v4l2_subdev_link_validate,
+};
+
+int vimc_ent_sd_register(struct vimc_ent_device *ved,
+ struct v4l2_subdev *sd,
+ struct v4l2_device *v4l2_dev,
+ const char *const name,
+ u32 function,
+ u16 num_pads,
+ struct media_pad *pads,
+ const struct v4l2_subdev_ops *sd_ops)
+{
+ int ret;
+
+ /* Fill the vimc_ent_device struct */
+ ved->ent = &sd->entity;
+
+ /* Initialize the subdev */
+ v4l2_subdev_init(sd, sd_ops);
+ sd->entity.function = function;
+ sd->entity.ops = &vimc_ent_sd_mops;
+ sd->owner = THIS_MODULE;
+ strscpy(sd->name, name, sizeof(sd->name));
+ v4l2_set_subdevdata(sd, ved);
+
+ /* Expose this subdev to user space */
+ sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
+ if (sd->ctrl_handler)
+ sd->flags |= V4L2_SUBDEV_FL_HAS_EVENTS;
+
+ /* Initialize the media entity */
+ ret = media_entity_pads_init(&sd->entity, num_pads, pads);
+ if (ret)
+ return ret;
+
+ /* Register the subdev with the v4l2 and the media framework */
+ ret = v4l2_device_register_subdev(v4l2_dev, sd);
+ if (ret) {
+ dev_err(v4l2_dev->dev,
+ "%s: subdev register failed (err=%d)\n",
+ name, ret);
+ goto err_clean_m_ent;
+ }
+
+ return 0;
+
+err_clean_m_ent:
+ media_entity_cleanup(&sd->entity);
+ return ret;
+}
diff --git a/drivers/media/test-drivers/vimc/vimc-common.h b/drivers/media/test-drivers/vimc/vimc-common.h
new file mode 100644
index 0000000..a289434
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/vimc-common.h
@@ -0,0 +1,233 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * vimc-common.h Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#ifndef _VIMC_COMMON_H_
+#define _VIMC_COMMON_H_
+
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <media/media-device.h>
+#include <media/v4l2-device.h>
+
+#define VIMC_PDEV_NAME "vimc"
+
+/* VIMC-specific controls */
+#define VIMC_CID_VIMC_BASE (0x00f00000 | 0xf000)
+#define VIMC_CID_VIMC_CLASS (0x00f00000 | 1)
+#define VIMC_CID_TEST_PATTERN (VIMC_CID_VIMC_BASE + 0)
+#define VIMC_CID_MEAN_WIN_SIZE (VIMC_CID_VIMC_BASE + 1)
+#define VIMC_CID_OSD_TEXT_MODE (VIMC_CID_VIMC_BASE + 2)
+
+#define VIMC_FRAME_MAX_WIDTH 4096
+#define VIMC_FRAME_MAX_HEIGHT 2160
+#define VIMC_FRAME_MIN_WIDTH 16
+#define VIMC_FRAME_MIN_HEIGHT 16
+
+#define VIMC_FRAME_INDEX(lin, col, width, bpp) ((lin * width + col) * bpp)
+
+/* Source and sink pad checks */
+#define VIMC_IS_SRC(pad) (pad)
+#define VIMC_IS_SINK(pad) (!(pad))
+
+#define VIMC_PIX_FMT_MAX_CODES 8
+
+/**
+ * vimc_colorimetry_clamp - Adjust colorimetry parameters
+ *
+ * @fmt: the pointer to struct v4l2_pix_format or
+ * struct v4l2_mbus_framefmt
+ *
+ * Entities must check if colorimetry given by the userspace is valid, if not
+ * then set them as DEFAULT
+ */
+#define vimc_colorimetry_clamp(fmt) \
+do { \
+ if ((fmt)->colorspace == V4L2_COLORSPACE_DEFAULT \
+ || (fmt)->colorspace > V4L2_COLORSPACE_DCI_P3) { \
+ (fmt)->colorspace = V4L2_COLORSPACE_DEFAULT; \
+ (fmt)->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT; \
+ (fmt)->quantization = V4L2_QUANTIZATION_DEFAULT; \
+ (fmt)->xfer_func = V4L2_XFER_FUNC_DEFAULT; \
+ } \
+ if ((fmt)->ycbcr_enc > V4L2_YCBCR_ENC_SMPTE240M) \
+ (fmt)->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT; \
+ if ((fmt)->quantization > V4L2_QUANTIZATION_LIM_RANGE) \
+ (fmt)->quantization = V4L2_QUANTIZATION_DEFAULT; \
+ if ((fmt)->xfer_func > V4L2_XFER_FUNC_SMPTE2084) \
+ (fmt)->xfer_func = V4L2_XFER_FUNC_DEFAULT; \
+} while (0)
+
+/**
+ * struct vimc_pix_map - maps media bus code with v4l2 pixel format
+ *
+ * @code: media bus format code defined by MEDIA_BUS_FMT_* macros
+ * @bpp: number of bytes each pixel occupies
+ * @pixelformat: pixel format defined by V4L2_PIX_FMT_* macros
+ * @bayer: true if this is a bayer format
+ *
+ * Struct which matches the MEDIA_BUS_FMT_* codes with the corresponding
+ * V4L2_PIX_FMT_* fourcc pixelformat and its bytes per pixel (bpp)
+ */
+struct vimc_pix_map {
+ unsigned int code[VIMC_PIX_FMT_MAX_CODES];
+ unsigned int bpp;
+ u32 pixelformat;
+ bool bayer;
+};
+
+/**
+ * struct vimc_ent_device - core struct that represents an entity in the
+ * topology
+ *
+ * @dev: a pointer of the device struct of the driver
+ * @ent: the pointer to struct media_entity for the node
+ * @process_frame: callback send a frame to that node
+ * @vdev_get_format: callback that returns the current format a pad, used
+ * only when is_media_entity_v4l2_video_device(ent) returns
+ * true
+ *
+ * Each node of the topology must create a vimc_ent_device struct. Depending on
+ * the node it will be of an instance of v4l2_subdev or video_device struct
+ * where both contains a struct media_entity.
+ * Those structures should embedded the vimc_ent_device struct through
+ * v4l2_set_subdevdata() and video_set_drvdata() respectively, allowing the
+ * vimc_ent_device struct to be retrieved from the corresponding struct
+ * media_entity
+ */
+struct vimc_ent_device {
+ struct device *dev;
+ struct media_entity *ent;
+ void * (*process_frame)(struct vimc_ent_device *ved,
+ const void *frame);
+ void (*vdev_get_format)(struct vimc_ent_device *ved,
+ struct v4l2_pix_format *fmt);
+};
+
+/**
+ * struct vimc_device - main device for vimc driver
+ *
+ * @pipe_cfg: pointer to the vimc pipeline configuration structure
+ * @ent_devs: array of vimc_ent_device pointers
+ * @mdev: the associated media_device parent
+ * @v4l2_dev: Internal v4l2 parent device
+ */
+struct vimc_device {
+ const struct vimc_pipeline_config *pipe_cfg;
+ struct vimc_ent_device **ent_devs;
+ struct media_device mdev;
+ struct v4l2_device v4l2_dev;
+};
+
+/**
+ * struct vimc_ent_type Structure for the callbacks of the entity types
+ *
+ *
+ * @add: initializes and registers
+ * vimc entity - called from vimc-core
+ * @unregister: unregisters vimc entity - called from vimc-core
+ * @release: releases vimc entity - called from the v4l2_dev
+ * release callback
+ */
+struct vimc_ent_type {
+ struct vimc_ent_device *(*add)(struct vimc_device *vimc,
+ const char *vcfg_name);
+ void (*unregister)(struct vimc_ent_device *ved);
+ void (*release)(struct vimc_ent_device *ved);
+};
+
+/**
+ * struct vimc_ent_config Structure which describes individual
+ * configuration for each entity
+ *
+ * @name: entity name
+ * @type: contain the callbacks of this entity type
+ *
+ */
+struct vimc_ent_config {
+ const char *name;
+ struct vimc_ent_type *type;
+};
+
+/**
+ * vimc_is_source - returns true if the entity has only source pads
+ *
+ * @ent: pointer to &struct media_entity
+ *
+ */
+bool vimc_is_source(struct media_entity *ent);
+
+extern struct vimc_ent_type vimc_sen_type;
+extern struct vimc_ent_type vimc_deb_type;
+extern struct vimc_ent_type vimc_sca_type;
+extern struct vimc_ent_type vimc_cap_type;
+
+/**
+ * vimc_pix_map_by_index - get vimc_pix_map struct by its index
+ *
+ * @i: index of the vimc_pix_map struct in vimc_pix_map_list
+ */
+const struct vimc_pix_map *vimc_pix_map_by_index(unsigned int i);
+
+/**
+ * vimc_mbus_code_by_index - get mbus code by its index
+ *
+ * @index: index of the mbus code in vimc_pix_map_list
+ *
+ * Returns 0 if no mbus code is found for the given index.
+ */
+u32 vimc_mbus_code_by_index(unsigned int index);
+
+/**
+ * vimc_pix_map_by_code - get vimc_pix_map struct by media bus code
+ *
+ * @code: media bus format code defined by MEDIA_BUS_FMT_* macros
+ */
+const struct vimc_pix_map *vimc_pix_map_by_code(u32 code);
+
+/**
+ * vimc_pix_map_by_pixelformat - get vimc_pix_map struct by v4l2 pixel format
+ *
+ * @pixelformat: pixel format defined by V4L2_PIX_FMT_* macros
+ */
+const struct vimc_pix_map *vimc_pix_map_by_pixelformat(u32 pixelformat);
+
+/**
+ * vimc_ent_sd_register - initialize and register a subdev node
+ *
+ * @ved: the vimc_ent_device struct to be initialize
+ * @sd: the v4l2_subdev struct to be initialize and registered
+ * @v4l2_dev: the v4l2 device to register the v4l2_subdev
+ * @name: name of the sub-device. Please notice that the name must be
+ * unique.
+ * @function: media entity function defined by MEDIA_ENT_F_* macros
+ * @num_pads: number of pads to initialize
+ * @pads: the array of pads of the entity, the caller should set the
+ * flags of the pads
+ * @sd_ops: pointer to &struct v4l2_subdev_ops.
+ *
+ * Helper function initialize and register the struct vimc_ent_device and struct
+ * v4l2_subdev which represents a subdev node in the topology
+ */
+int vimc_ent_sd_register(struct vimc_ent_device *ved,
+ struct v4l2_subdev *sd,
+ struct v4l2_device *v4l2_dev,
+ const char *const name,
+ u32 function,
+ u16 num_pads,
+ struct media_pad *pads,
+ const struct v4l2_subdev_ops *sd_ops);
+
+/**
+ * vimc_vdev_link_validate - validates a media link
+ *
+ * @link: pointer to &struct media_link
+ *
+ * This function calls validates if a media link is valid for streaming.
+ */
+int vimc_vdev_link_validate(struct media_link *link);
+
+#endif
diff --git a/drivers/media/test-drivers/vimc/vimc-core.c b/drivers/media/test-drivers/vimc/vimc-core.c
new file mode 100644
index 0000000..4b0ae6f
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/vimc-core.c
@@ -0,0 +1,379 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-core.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/font.h>
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <media/media-device.h>
+#include <media/tpg/v4l2-tpg.h>
+#include <media/v4l2-device.h>
+
+#include "vimc-common.h"
+
+#define VIMC_MDEV_MODEL_NAME "VIMC MDEV"
+
+#define VIMC_ENT_LINK(src, srcpad, sink, sinkpad, link_flags) { \
+ .src_ent = src, \
+ .src_pad = srcpad, \
+ .sink_ent = sink, \
+ .sink_pad = sinkpad, \
+ .flags = link_flags, \
+}
+
+/* Structure which describes links between entities */
+struct vimc_ent_link {
+ unsigned int src_ent;
+ u16 src_pad;
+ unsigned int sink_ent;
+ u16 sink_pad;
+ u32 flags;
+};
+
+/* Structure which describes the whole topology */
+struct vimc_pipeline_config {
+ const struct vimc_ent_config *ents;
+ size_t num_ents;
+ const struct vimc_ent_link *links;
+ size_t num_links;
+};
+
+/* --------------------------------------------------------------------------
+ * Topology Configuration
+ */
+
+static struct vimc_ent_config ent_config[] = {
+ {
+ .name = "Sensor A",
+ .type = &vimc_sen_type
+ },
+ {
+ .name = "Sensor B",
+ .type = &vimc_sen_type
+ },
+ {
+ .name = "Debayer A",
+ .type = &vimc_deb_type
+ },
+ {
+ .name = "Debayer B",
+ .type = &vimc_deb_type
+ },
+ {
+ .name = "Raw Capture 0",
+ .type = &vimc_cap_type
+ },
+ {
+ .name = "Raw Capture 1",
+ .type = &vimc_cap_type
+ },
+ {
+ /* TODO: change this to vimc-input when it is implemented */
+ .name = "RGB/YUV Input",
+ .type = &vimc_sen_type
+ },
+ {
+ .name = "Scaler",
+ .type = &vimc_sca_type
+ },
+ {
+ .name = "RGB/YUV Capture",
+ .type = &vimc_cap_type
+ },
+};
+
+static const struct vimc_ent_link ent_links[] = {
+ /* Link: Sensor A (Pad 0)->(Pad 0) Debayer A */
+ VIMC_ENT_LINK(0, 0, 2, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+ /* Link: Sensor A (Pad 0)->(Pad 0) Raw Capture 0 */
+ VIMC_ENT_LINK(0, 0, 4, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+ /* Link: Sensor B (Pad 0)->(Pad 0) Debayer B */
+ VIMC_ENT_LINK(1, 0, 3, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+ /* Link: Sensor B (Pad 0)->(Pad 0) Raw Capture 1 */
+ VIMC_ENT_LINK(1, 0, 5, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+ /* Link: Debayer A (Pad 1)->(Pad 0) Scaler */
+ VIMC_ENT_LINK(2, 1, 7, 0, MEDIA_LNK_FL_ENABLED),
+ /* Link: Debayer B (Pad 1)->(Pad 0) Scaler */
+ VIMC_ENT_LINK(3, 1, 7, 0, 0),
+ /* Link: RGB/YUV Input (Pad 0)->(Pad 0) Scaler */
+ VIMC_ENT_LINK(6, 0, 7, 0, 0),
+ /* Link: Scaler (Pad 1)->(Pad 0) RGB/YUV Capture */
+ VIMC_ENT_LINK(7, 1, 8, 0, MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE),
+};
+
+static struct vimc_pipeline_config pipe_cfg = {
+ .ents = ent_config,
+ .num_ents = ARRAY_SIZE(ent_config),
+ .links = ent_links,
+ .num_links = ARRAY_SIZE(ent_links)
+};
+
+/* -------------------------------------------------------------------------- */
+
+static void vimc_rm_links(struct vimc_device *vimc)
+{
+ unsigned int i;
+
+ for (i = 0; i < vimc->pipe_cfg->num_ents; i++)
+ media_entity_remove_links(vimc->ent_devs[i]->ent);
+}
+
+static int vimc_create_links(struct vimc_device *vimc)
+{
+ unsigned int i;
+ int ret;
+
+ /* Initialize the links between entities */
+ for (i = 0; i < vimc->pipe_cfg->num_links; i++) {
+ const struct vimc_ent_link *link = &vimc->pipe_cfg->links[i];
+
+ struct vimc_ent_device *ved_src =
+ vimc->ent_devs[link->src_ent];
+ struct vimc_ent_device *ved_sink =
+ vimc->ent_devs[link->sink_ent];
+
+ ret = media_create_pad_link(ved_src->ent, link->src_pad,
+ ved_sink->ent, link->sink_pad,
+ link->flags);
+ if (ret)
+ goto err_rm_links;
+ }
+
+ return 0;
+
+err_rm_links:
+ vimc_rm_links(vimc);
+ return ret;
+}
+
+static void vimc_release_subdevs(struct vimc_device *vimc)
+{
+ unsigned int i;
+
+ for (i = 0; i < vimc->pipe_cfg->num_ents; i++)
+ if (vimc->ent_devs[i])
+ vimc->pipe_cfg->ents[i].type->release(vimc->ent_devs[i]);
+}
+
+static void vimc_unregister_subdevs(struct vimc_device *vimc)
+{
+ unsigned int i;
+
+ for (i = 0; i < vimc->pipe_cfg->num_ents; i++)
+ if (vimc->ent_devs[i] && vimc->pipe_cfg->ents[i].type->unregister)
+ vimc->pipe_cfg->ents[i].type->unregister(vimc->ent_devs[i]);
+}
+
+static int vimc_add_subdevs(struct vimc_device *vimc)
+{
+ unsigned int i;
+
+ for (i = 0; i < vimc->pipe_cfg->num_ents; i++) {
+ dev_dbg(vimc->mdev.dev, "new entity for %s\n",
+ vimc->pipe_cfg->ents[i].name);
+ vimc->ent_devs[i] = vimc->pipe_cfg->ents[i].type->add(vimc,
+ vimc->pipe_cfg->ents[i].name);
+ if (IS_ERR(vimc->ent_devs[i])) {
+ int err = PTR_ERR(vimc->ent_devs[i]);
+
+ dev_err(vimc->mdev.dev, "adding entity %s failed (%d)\n",
+ vimc->pipe_cfg->ents[i].name, err);
+ vimc->ent_devs[i] = NULL;
+ vimc_unregister_subdevs(vimc);
+ vimc_release_subdevs(vimc);
+ return err;
+ }
+ }
+ return 0;
+}
+
+static void vimc_v4l2_dev_release(struct v4l2_device *v4l2_dev)
+{
+ struct vimc_device *vimc =
+ container_of(v4l2_dev, struct vimc_device, v4l2_dev);
+
+ vimc_release_subdevs(vimc);
+ media_device_cleanup(&vimc->mdev);
+ kfree(vimc->ent_devs);
+ kfree(vimc);
+}
+
+static int vimc_register_devices(struct vimc_device *vimc)
+{
+ int ret;
+
+ /* Register the v4l2 struct */
+ ret = v4l2_device_register(vimc->mdev.dev, &vimc->v4l2_dev);
+ if (ret) {
+ dev_err(vimc->mdev.dev,
+ "v4l2 device register failed (err=%d)\n", ret);
+ return ret;
+ }
+ /* allocate ent_devs */
+ vimc->ent_devs = kcalloc(vimc->pipe_cfg->num_ents,
+ sizeof(*vimc->ent_devs), GFP_KERNEL);
+ if (!vimc->ent_devs) {
+ ret = -ENOMEM;
+ goto err_v4l2_unregister;
+ }
+
+ /* Invoke entity config hooks to initialize and register subdevs */
+ ret = vimc_add_subdevs(vimc);
+ if (ret)
+ goto err_free_ent_devs;
+
+ /* Initialize links */
+ ret = vimc_create_links(vimc);
+ if (ret)
+ goto err_rm_subdevs;
+
+ /* Register the media device */
+ ret = media_device_register(&vimc->mdev);
+ if (ret) {
+ dev_err(vimc->mdev.dev,
+ "media device register failed (err=%d)\n", ret);
+ goto err_rm_subdevs;
+ }
+
+ /* Expose all subdev's nodes*/
+ ret = v4l2_device_register_subdev_nodes(&vimc->v4l2_dev);
+ if (ret) {
+ dev_err(vimc->mdev.dev,
+ "vimc subdev nodes registration failed (err=%d)\n",
+ ret);
+ goto err_mdev_unregister;
+ }
+
+ return 0;
+
+err_mdev_unregister:
+ media_device_unregister(&vimc->mdev);
+err_rm_subdevs:
+ vimc_unregister_subdevs(vimc);
+ vimc_release_subdevs(vimc);
+err_free_ent_devs:
+ kfree(vimc->ent_devs);
+err_v4l2_unregister:
+ v4l2_device_unregister(&vimc->v4l2_dev);
+
+ return ret;
+}
+
+static int vimc_probe(struct platform_device *pdev)
+{
+ const struct font_desc *font = find_font("VGA8x16");
+ struct vimc_device *vimc;
+ int ret;
+
+ dev_dbg(&pdev->dev, "probe");
+
+ if (!font) {
+ dev_err(&pdev->dev, "could not find font\n");
+ return -ENODEV;
+ }
+
+ tpg_set_font(font->data);
+
+ vimc = kzalloc(sizeof(*vimc), GFP_KERNEL);
+ if (!vimc)
+ return -ENOMEM;
+
+ vimc->pipe_cfg = &pipe_cfg;
+
+ /* Link the media device within the v4l2_device */
+ vimc->v4l2_dev.mdev = &vimc->mdev;
+
+ /* Initialize media device */
+ strscpy(vimc->mdev.model, VIMC_MDEV_MODEL_NAME,
+ sizeof(vimc->mdev.model));
+ snprintf(vimc->mdev.bus_info, sizeof(vimc->mdev.bus_info),
+ "platform:%s", VIMC_PDEV_NAME);
+ vimc->mdev.dev = &pdev->dev;
+ media_device_init(&vimc->mdev);
+
+ ret = vimc_register_devices(vimc);
+ if (ret) {
+ media_device_cleanup(&vimc->mdev);
+ kfree(vimc);
+ return ret;
+ }
+ /*
+ * the release cb is set only after successful registration.
+ * if the registration fails, we release directly from probe
+ */
+
+ vimc->v4l2_dev.release = vimc_v4l2_dev_release;
+ platform_set_drvdata(pdev, vimc);
+ return 0;
+}
+
+static int vimc_remove(struct platform_device *pdev)
+{
+ struct vimc_device *vimc = platform_get_drvdata(pdev);
+
+ dev_dbg(&pdev->dev, "remove");
+
+ vimc_unregister_subdevs(vimc);
+ media_device_unregister(&vimc->mdev);
+ v4l2_device_unregister(&vimc->v4l2_dev);
+ v4l2_device_put(&vimc->v4l2_dev);
+
+ return 0;
+}
+
+static void vimc_dev_release(struct device *dev)
+{
+}
+
+static struct platform_device vimc_pdev = {
+ .name = VIMC_PDEV_NAME,
+ .dev.release = vimc_dev_release,
+};
+
+static struct platform_driver vimc_pdrv = {
+ .probe = vimc_probe,
+ .remove = vimc_remove,
+ .driver = {
+ .name = VIMC_PDEV_NAME,
+ },
+};
+
+static int __init vimc_init(void)
+{
+ int ret;
+
+ ret = platform_device_register(&vimc_pdev);
+ if (ret) {
+ dev_err(&vimc_pdev.dev,
+ "platform device registration failed (err=%d)\n", ret);
+ return ret;
+ }
+
+ ret = platform_driver_register(&vimc_pdrv);
+ if (ret) {
+ dev_err(&vimc_pdev.dev,
+ "platform driver registration failed (err=%d)\n", ret);
+ platform_driver_unregister(&vimc_pdrv);
+ return ret;
+ }
+
+ return 0;
+}
+
+static void __exit vimc_exit(void)
+{
+ platform_driver_unregister(&vimc_pdrv);
+
+ platform_device_unregister(&vimc_pdev);
+}
+
+module_init(vimc_init);
+module_exit(vimc_exit);
+
+MODULE_DESCRIPTION("Virtual Media Controller Driver (VIMC)");
+MODULE_AUTHOR("Helen Fornazier <helen.fornazier@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/test-drivers/vimc/vimc-debayer.c b/drivers/media/test-drivers/vimc/vimc-debayer.c
new file mode 100644
index 0000000..c3f6fef
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/vimc-debayer.c
@@ -0,0 +1,623 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-debayer.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/moduleparam.h>
+#include <linux/platform_device.h>
+#include <linux/vmalloc.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-subdev.h>
+
+#include "vimc-common.h"
+
+enum vimc_deb_rgb_colors {
+ VIMC_DEB_RED = 0,
+ VIMC_DEB_GREEN = 1,
+ VIMC_DEB_BLUE = 2,
+};
+
+struct vimc_deb_pix_map {
+ u32 code;
+ enum vimc_deb_rgb_colors order[2][2];
+};
+
+struct vimc_deb_device {
+ struct vimc_ent_device ved;
+ struct v4l2_subdev sd;
+ /* The active format */
+ struct v4l2_mbus_framefmt sink_fmt;
+ u32 src_code;
+ void (*set_rgb_src)(struct vimc_deb_device *vdeb, unsigned int lin,
+ unsigned int col, unsigned int rgb[3]);
+ /* Values calculated when the stream starts */
+ u8 *src_frame;
+ const struct vimc_deb_pix_map *sink_pix_map;
+ unsigned int sink_bpp;
+ unsigned int mean_win_size;
+ struct v4l2_ctrl_handler hdl;
+ struct media_pad pads[2];
+};
+
+static const struct v4l2_mbus_framefmt sink_fmt_default = {
+ .width = 640,
+ .height = 480,
+ .code = MEDIA_BUS_FMT_SRGGB8_1X8,
+ .field = V4L2_FIELD_NONE,
+ .colorspace = V4L2_COLORSPACE_SRGB,
+};
+
+static const u32 vimc_deb_src_mbus_codes[] = {
+ MEDIA_BUS_FMT_GBR888_1X24,
+ MEDIA_BUS_FMT_BGR888_1X24,
+ MEDIA_BUS_FMT_BGR888_3X8,
+ MEDIA_BUS_FMT_RGB888_1X24,
+ MEDIA_BUS_FMT_RGB888_2X12_BE,
+ MEDIA_BUS_FMT_RGB888_2X12_LE,
+ MEDIA_BUS_FMT_RGB888_3X8,
+ MEDIA_BUS_FMT_RGB888_1X7X4_SPWG,
+ MEDIA_BUS_FMT_RGB888_1X7X4_JEIDA,
+ MEDIA_BUS_FMT_RGB888_1X32_PADHI,
+};
+
+static const struct vimc_deb_pix_map vimc_deb_pix_map_list[] = {
+ {
+ .code = MEDIA_BUS_FMT_SBGGR8_1X8,
+ .order = { { VIMC_DEB_BLUE, VIMC_DEB_GREEN },
+ { VIMC_DEB_GREEN, VIMC_DEB_RED } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SGBRG8_1X8,
+ .order = { { VIMC_DEB_GREEN, VIMC_DEB_BLUE },
+ { VIMC_DEB_RED, VIMC_DEB_GREEN } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SGRBG8_1X8,
+ .order = { { VIMC_DEB_GREEN, VIMC_DEB_RED },
+ { VIMC_DEB_BLUE, VIMC_DEB_GREEN } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SRGGB8_1X8,
+ .order = { { VIMC_DEB_RED, VIMC_DEB_GREEN },
+ { VIMC_DEB_GREEN, VIMC_DEB_BLUE } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SBGGR10_1X10,
+ .order = { { VIMC_DEB_BLUE, VIMC_DEB_GREEN },
+ { VIMC_DEB_GREEN, VIMC_DEB_RED } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SGBRG10_1X10,
+ .order = { { VIMC_DEB_GREEN, VIMC_DEB_BLUE },
+ { VIMC_DEB_RED, VIMC_DEB_GREEN } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SGRBG10_1X10,
+ .order = { { VIMC_DEB_GREEN, VIMC_DEB_RED },
+ { VIMC_DEB_BLUE, VIMC_DEB_GREEN } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SRGGB10_1X10,
+ .order = { { VIMC_DEB_RED, VIMC_DEB_GREEN },
+ { VIMC_DEB_GREEN, VIMC_DEB_BLUE } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SBGGR12_1X12,
+ .order = { { VIMC_DEB_BLUE, VIMC_DEB_GREEN },
+ { VIMC_DEB_GREEN, VIMC_DEB_RED } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SGBRG12_1X12,
+ .order = { { VIMC_DEB_GREEN, VIMC_DEB_BLUE },
+ { VIMC_DEB_RED, VIMC_DEB_GREEN } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SGRBG12_1X12,
+ .order = { { VIMC_DEB_GREEN, VIMC_DEB_RED },
+ { VIMC_DEB_BLUE, VIMC_DEB_GREEN } }
+ },
+ {
+ .code = MEDIA_BUS_FMT_SRGGB12_1X12,
+ .order = { { VIMC_DEB_RED, VIMC_DEB_GREEN },
+ { VIMC_DEB_GREEN, VIMC_DEB_BLUE } }
+ },
+};
+
+static const struct vimc_deb_pix_map *vimc_deb_pix_map_by_code(u32 code)
+{
+ unsigned int i;
+
+ for (i = 0; i < ARRAY_SIZE(vimc_deb_pix_map_list); i++)
+ if (vimc_deb_pix_map_list[i].code == code)
+ return &vimc_deb_pix_map_list[i];
+
+ return NULL;
+}
+
+static bool vimc_deb_src_code_is_valid(u32 code)
+{
+ unsigned int i;
+
+ for (i = 0; i < ARRAY_SIZE(vimc_deb_src_mbus_codes); i++)
+ if (vimc_deb_src_mbus_codes[i] == code)
+ return true;
+
+ return false;
+}
+
+static int vimc_deb_init_cfg(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg)
+{
+ struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *mf;
+ unsigned int i;
+
+ mf = v4l2_subdev_get_try_format(sd, cfg, 0);
+ *mf = sink_fmt_default;
+
+ for (i = 1; i < sd->entity.num_pads; i++) {
+ mf = v4l2_subdev_get_try_format(sd, cfg, i);
+ *mf = sink_fmt_default;
+ mf->code = vdeb->src_code;
+ }
+
+ return 0;
+}
+
+static int vimc_deb_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ if (VIMC_IS_SRC(code->pad)) {
+ if (code->index >= ARRAY_SIZE(vimc_deb_src_mbus_codes))
+ return -EINVAL;
+
+ code->code = vimc_deb_src_mbus_codes[code->index];
+ } else {
+ if (code->index >= ARRAY_SIZE(vimc_deb_pix_map_list))
+ return -EINVAL;
+
+ code->code = vimc_deb_pix_map_list[code->index].code;
+ }
+
+ return 0;
+}
+
+static int vimc_deb_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ if (fse->index)
+ return -EINVAL;
+
+ if (VIMC_IS_SINK(fse->pad)) {
+ const struct vimc_deb_pix_map *vpix =
+ vimc_deb_pix_map_by_code(fse->code);
+
+ if (!vpix)
+ return -EINVAL;
+ } else if (!vimc_deb_src_code_is_valid(fse->code)) {
+ return -EINVAL;
+ }
+
+ fse->min_width = VIMC_FRAME_MIN_WIDTH;
+ fse->max_width = VIMC_FRAME_MAX_WIDTH;
+ fse->min_height = VIMC_FRAME_MIN_HEIGHT;
+ fse->max_height = VIMC_FRAME_MAX_HEIGHT;
+
+ return 0;
+}
+
+static int vimc_deb_get_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *fmt)
+{
+ struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+
+ /* Get the current sink format */
+ fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
+ *v4l2_subdev_get_try_format(sd, cfg, 0) :
+ vdeb->sink_fmt;
+
+ /* Set the right code for the source pad */
+ if (VIMC_IS_SRC(fmt->pad))
+ fmt->format.code = vdeb->src_code;
+
+ return 0;
+}
+
+static void vimc_deb_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)
+{
+ const struct vimc_deb_pix_map *vpix;
+
+ /* Don't accept a code that is not on the debayer table */
+ vpix = vimc_deb_pix_map_by_code(fmt->code);
+ if (!vpix)
+ fmt->code = sink_fmt_default.code;
+
+ fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
+ VIMC_FRAME_MAX_WIDTH) & ~1;
+ fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
+ VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+ if (fmt->field == V4L2_FIELD_ANY)
+ fmt->field = sink_fmt_default.field;
+
+ vimc_colorimetry_clamp(fmt);
+}
+
+static int vimc_deb_set_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *fmt)
+{
+ struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *sink_fmt;
+ u32 *src_code;
+
+ if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+ /* Do not change the format while stream is on */
+ if (vdeb->src_frame)
+ return -EBUSY;
+
+ sink_fmt = &vdeb->sink_fmt;
+ src_code = &vdeb->src_code;
+ } else {
+ sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+ src_code = &v4l2_subdev_get_try_format(sd, cfg, 1)->code;
+ }
+
+ /*
+ * Do not change the format of the source pad,
+ * it is propagated from the sink
+ */
+ if (VIMC_IS_SRC(fmt->pad)) {
+ u32 code = fmt->format.code;
+
+ fmt->format = *sink_fmt;
+
+ if (vimc_deb_src_code_is_valid(code))
+ *src_code = code;
+
+ fmt->format.code = *src_code;
+ } else {
+ /* Set the new format in the sink pad */
+ vimc_deb_adjust_sink_fmt(&fmt->format);
+
+ dev_dbg(vdeb->ved.dev, "%s: sink format update: "
+ "old:%dx%d (0x%x, %d, %d, %d, %d) "
+ "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vdeb->sd.name,
+ /* old */
+ sink_fmt->width, sink_fmt->height, sink_fmt->code,
+ sink_fmt->colorspace, sink_fmt->quantization,
+ sink_fmt->xfer_func, sink_fmt->ycbcr_enc,
+ /* new */
+ fmt->format.width, fmt->format.height, fmt->format.code,
+ fmt->format.colorspace, fmt->format.quantization,
+ fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+ *sink_fmt = fmt->format;
+ }
+
+ return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_deb_pad_ops = {
+ .init_cfg = vimc_deb_init_cfg,
+ .enum_mbus_code = vimc_deb_enum_mbus_code,
+ .enum_frame_size = vimc_deb_enum_frame_size,
+ .get_fmt = vimc_deb_get_fmt,
+ .set_fmt = vimc_deb_set_fmt,
+};
+
+static void vimc_deb_process_rgb_frame(struct vimc_deb_device *vdeb,
+ unsigned int lin,
+ unsigned int col,
+ unsigned int rgb[3])
+{
+ const struct vimc_pix_map *vpix;
+ unsigned int i, index;
+
+ vpix = vimc_pix_map_by_code(vdeb->src_code);
+ index = VIMC_FRAME_INDEX(lin, col, vdeb->sink_fmt.width, 3);
+ for (i = 0; i < 3; i++) {
+ switch (vpix->pixelformat) {
+ case V4L2_PIX_FMT_RGB24:
+ vdeb->src_frame[index + i] = rgb[i];
+ break;
+ case V4L2_PIX_FMT_BGR24:
+ vdeb->src_frame[index + i] = rgb[2 - i];
+ break;
+ }
+ }
+}
+
+static int vimc_deb_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct vimc_deb_device *vdeb = v4l2_get_subdevdata(sd);
+
+ if (enable) {
+ const struct vimc_pix_map *vpix;
+ unsigned int frame_size;
+
+ if (vdeb->src_frame)
+ return 0;
+
+ /* Calculate the frame size of the source pad */
+ vpix = vimc_pix_map_by_code(vdeb->src_code);
+ frame_size = vdeb->sink_fmt.width * vdeb->sink_fmt.height *
+ vpix->bpp;
+
+ /* Save the bytes per pixel of the sink */
+ vpix = vimc_pix_map_by_code(vdeb->sink_fmt.code);
+ vdeb->sink_bpp = vpix->bpp;
+
+ /* Get the corresponding pixel map from the table */
+ vdeb->sink_pix_map =
+ vimc_deb_pix_map_by_code(vdeb->sink_fmt.code);
+
+ /*
+ * Allocate the frame buffer. Use vmalloc to be able to
+ * allocate a large amount of memory
+ */
+ vdeb->src_frame = vmalloc(frame_size);
+ if (!vdeb->src_frame)
+ return -ENOMEM;
+
+ } else {
+ if (!vdeb->src_frame)
+ return 0;
+
+ vfree(vdeb->src_frame);
+ vdeb->src_frame = NULL;
+ }
+
+ return 0;
+}
+
+static const struct v4l2_subdev_core_ops vimc_deb_core_ops = {
+ .log_status = v4l2_ctrl_subdev_log_status,
+ .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+ .unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops vimc_deb_video_ops = {
+ .s_stream = vimc_deb_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_deb_ops = {
+ .core = &vimc_deb_core_ops,
+ .pad = &vimc_deb_pad_ops,
+ .video = &vimc_deb_video_ops,
+};
+
+static unsigned int vimc_deb_get_val(const u8 *bytes,
+ const unsigned int n_bytes)
+{
+ unsigned int i;
+ unsigned int acc = 0;
+
+ for (i = 0; i < n_bytes; i++)
+ acc = acc + (bytes[i] << (8 * i));
+
+ return acc;
+}
+
+static void vimc_deb_calc_rgb_sink(struct vimc_deb_device *vdeb,
+ const u8 *frame,
+ const unsigned int lin,
+ const unsigned int col,
+ unsigned int rgb[3])
+{
+ unsigned int i, seek, wlin, wcol;
+ unsigned int n_rgb[3] = {0, 0, 0};
+
+ for (i = 0; i < 3; i++)
+ rgb[i] = 0;
+
+ /*
+ * Calculate how many we need to subtract to get to the pixel in
+ * the top left corner of the mean window (considering the current
+ * pixel as the center)
+ */
+ seek = vdeb->mean_win_size / 2;
+
+ /* Sum the values of the colors in the mean window */
+
+ dev_dbg(vdeb->ved.dev,
+ "deb: %s: --- Calc pixel %dx%d, window mean %d, seek %d ---\n",
+ vdeb->sd.name, lin, col, vdeb->sink_fmt.height, seek);
+
+ /*
+ * Iterate through all the lines in the mean window, start
+ * with zero if the pixel is outside the frame and don't pass
+ * the height when the pixel is in the bottom border of the
+ * frame
+ */
+ for (wlin = seek > lin ? 0 : lin - seek;
+ wlin < lin + seek + 1 && wlin < vdeb->sink_fmt.height;
+ wlin++) {
+
+ /*
+ * Iterate through all the columns in the mean window, start
+ * with zero if the pixel is outside the frame and don't pass
+ * the width when the pixel is in the right border of the
+ * frame
+ */
+ for (wcol = seek > col ? 0 : col - seek;
+ wcol < col + seek + 1 && wcol < vdeb->sink_fmt.width;
+ wcol++) {
+ enum vimc_deb_rgb_colors color;
+ unsigned int index;
+
+ /* Check which color this pixel is */
+ color = vdeb->sink_pix_map->order[wlin % 2][wcol % 2];
+
+ index = VIMC_FRAME_INDEX(wlin, wcol,
+ vdeb->sink_fmt.width,
+ vdeb->sink_bpp);
+
+ dev_dbg(vdeb->ved.dev,
+ "deb: %s: RGB CALC: frame index %d, win pos %dx%d, color %d\n",
+ vdeb->sd.name, index, wlin, wcol, color);
+
+ /* Get its value */
+ rgb[color] = rgb[color] +
+ vimc_deb_get_val(&frame[index], vdeb->sink_bpp);
+
+ /* Save how many values we already added */
+ n_rgb[color]++;
+
+ dev_dbg(vdeb->ved.dev, "deb: %s: RGB CALC: val %d, n %d\n",
+ vdeb->sd.name, rgb[color], n_rgb[color]);
+ }
+ }
+
+ /* Calculate the mean */
+ for (i = 0; i < 3; i++) {
+ dev_dbg(vdeb->ved.dev,
+ "deb: %s: PRE CALC: %dx%d Color %d, val %d, n %d\n",
+ vdeb->sd.name, lin, col, i, rgb[i], n_rgb[i]);
+
+ if (n_rgb[i])
+ rgb[i] = rgb[i] / n_rgb[i];
+
+ dev_dbg(vdeb->ved.dev,
+ "deb: %s: FINAL CALC: %dx%d Color %d, val %d\n",
+ vdeb->sd.name, lin, col, i, rgb[i]);
+ }
+}
+
+static void *vimc_deb_process_frame(struct vimc_ent_device *ved,
+ const void *sink_frame)
+{
+ struct vimc_deb_device *vdeb = container_of(ved, struct vimc_deb_device,
+ ved);
+ unsigned int rgb[3];
+ unsigned int i, j;
+
+ /* If the stream in this node is not active, just return */
+ if (!vdeb->src_frame)
+ return ERR_PTR(-EINVAL);
+
+ for (i = 0; i < vdeb->sink_fmt.height; i++)
+ for (j = 0; j < vdeb->sink_fmt.width; j++) {
+ vimc_deb_calc_rgb_sink(vdeb, sink_frame, i, j, rgb);
+ vdeb->set_rgb_src(vdeb, i, j, rgb);
+ }
+
+ return vdeb->src_frame;
+}
+
+static int vimc_deb_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vimc_deb_device *vdeb =
+ container_of(ctrl->handler, struct vimc_deb_device, hdl);
+
+ switch (ctrl->id) {
+ case VIMC_CID_MEAN_WIN_SIZE:
+ vdeb->mean_win_size = ctrl->val;
+ break;
+ default:
+ return -EINVAL;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vimc_deb_ctrl_ops = {
+ .s_ctrl = vimc_deb_s_ctrl,
+};
+
+static void vimc_deb_release(struct vimc_ent_device *ved)
+{
+ struct vimc_deb_device *vdeb =
+ container_of(ved, struct vimc_deb_device, ved);
+
+ v4l2_ctrl_handler_free(&vdeb->hdl);
+ media_entity_cleanup(vdeb->ved.ent);
+ kfree(vdeb);
+}
+
+static const struct v4l2_ctrl_config vimc_deb_ctrl_class = {
+ .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
+ .id = VIMC_CID_VIMC_CLASS,
+ .name = "VIMC Controls",
+ .type = V4L2_CTRL_TYPE_CTRL_CLASS,
+};
+
+static const struct v4l2_ctrl_config vimc_deb_ctrl_mean_win_size = {
+ .ops = &vimc_deb_ctrl_ops,
+ .id = VIMC_CID_MEAN_WIN_SIZE,
+ .name = "Debayer Mean Window Size",
+ .type = V4L2_CTRL_TYPE_INTEGER,
+ .min = 1,
+ .max = 25,
+ .step = 2,
+ .def = 3,
+};
+
+static struct vimc_ent_device *vimc_deb_add(struct vimc_device *vimc,
+ const char *vcfg_name)
+{
+ struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
+ struct vimc_deb_device *vdeb;
+ int ret;
+
+ /* Allocate the vdeb struct */
+ vdeb = kzalloc(sizeof(*vdeb), GFP_KERNEL);
+ if (!vdeb)
+ return ERR_PTR(-ENOMEM);
+
+ /* Create controls: */
+ v4l2_ctrl_handler_init(&vdeb->hdl, 2);
+ v4l2_ctrl_new_custom(&vdeb->hdl, &vimc_deb_ctrl_class, NULL);
+ v4l2_ctrl_new_custom(&vdeb->hdl, &vimc_deb_ctrl_mean_win_size, NULL);
+ vdeb->sd.ctrl_handler = &vdeb->hdl;
+ if (vdeb->hdl.error) {
+ ret = vdeb->hdl.error;
+ goto err_free_vdeb;
+ }
+
+ /* Initialize ved and sd */
+ vdeb->pads[0].flags = MEDIA_PAD_FL_SINK;
+ vdeb->pads[1].flags = MEDIA_PAD_FL_SOURCE;
+
+ ret = vimc_ent_sd_register(&vdeb->ved, &vdeb->sd, v4l2_dev,
+ vcfg_name,
+ MEDIA_ENT_F_PROC_VIDEO_PIXEL_ENC_CONV, 2,
+ vdeb->pads, &vimc_deb_ops);
+ if (ret)
+ goto err_free_hdl;
+
+ vdeb->ved.process_frame = vimc_deb_process_frame;
+ vdeb->ved.dev = vimc->mdev.dev;
+ vdeb->mean_win_size = vimc_deb_ctrl_mean_win_size.def;
+
+ /* Initialize the frame format */
+ vdeb->sink_fmt = sink_fmt_default;
+ /*
+ * TODO: Add support for more output formats, we only support
+ * RGB888 for now
+ * NOTE: the src format is always the same as the sink, except
+ * for the code
+ */
+ vdeb->src_code = MEDIA_BUS_FMT_RGB888_1X24;
+ vdeb->set_rgb_src = vimc_deb_process_rgb_frame;
+
+ return &vdeb->ved;
+
+err_free_hdl:
+ v4l2_ctrl_handler_free(&vdeb->hdl);
+err_free_vdeb:
+ kfree(vdeb);
+
+ return ERR_PTR(ret);
+}
+
+struct vimc_ent_type vimc_deb_type = {
+ .add = vimc_deb_add,
+ .release = vimc_deb_release
+};
diff --git a/drivers/media/test-drivers/vimc/vimc-scaler.c b/drivers/media/test-drivers/vimc/vimc-scaler.c
new file mode 100644
index 0000000..121fa7d
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/vimc-scaler.c
@@ -0,0 +1,522 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-scaler.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/moduleparam.h>
+#include <linux/vmalloc.h>
+#include <linux/v4l2-mediabus.h>
+#include <media/v4l2-rect.h>
+#include <media/v4l2-subdev.h>
+
+#include "vimc-common.h"
+
+static unsigned int sca_mult = 3;
+module_param(sca_mult, uint, 0000);
+MODULE_PARM_DESC(sca_mult, " the image size multiplier");
+
+#define MAX_ZOOM 8
+
+#define VIMC_SCA_FMT_WIDTH_DEFAULT 640
+#define VIMC_SCA_FMT_HEIGHT_DEFAULT 480
+
+struct vimc_sca_device {
+ struct vimc_ent_device ved;
+ struct v4l2_subdev sd;
+ /* NOTE: the source fmt is the same as the sink
+ * with the width and hight multiplied by mult
+ */
+ struct v4l2_mbus_framefmt sink_fmt;
+ struct v4l2_rect crop_rect;
+ /* Values calculated when the stream starts */
+ u8 *src_frame;
+ unsigned int src_line_size;
+ unsigned int bpp;
+ struct media_pad pads[2];
+};
+
+static const struct v4l2_mbus_framefmt sink_fmt_default = {
+ .width = VIMC_SCA_FMT_WIDTH_DEFAULT,
+ .height = VIMC_SCA_FMT_HEIGHT_DEFAULT,
+ .code = MEDIA_BUS_FMT_RGB888_1X24,
+ .field = V4L2_FIELD_NONE,
+ .colorspace = V4L2_COLORSPACE_SRGB,
+};
+
+static const struct v4l2_rect crop_rect_default = {
+ .width = VIMC_SCA_FMT_WIDTH_DEFAULT,
+ .height = VIMC_SCA_FMT_HEIGHT_DEFAULT,
+ .top = 0,
+ .left = 0,
+};
+
+static const struct v4l2_rect crop_rect_min = {
+ .width = VIMC_FRAME_MIN_WIDTH,
+ .height = VIMC_FRAME_MIN_HEIGHT,
+ .top = 0,
+ .left = 0,
+};
+
+static struct v4l2_rect
+vimc_sca_get_crop_bound_sink(const struct v4l2_mbus_framefmt *sink_fmt)
+{
+ /* Get the crop bounds to clamp the crop rectangle correctly */
+ struct v4l2_rect r = {
+ .left = 0,
+ .top = 0,
+ .width = sink_fmt->width,
+ .height = sink_fmt->height,
+ };
+ return r;
+}
+
+static void vimc_sca_adjust_sink_crop(struct v4l2_rect *r,
+ const struct v4l2_mbus_framefmt *sink_fmt)
+{
+ const struct v4l2_rect sink_rect =
+ vimc_sca_get_crop_bound_sink(sink_fmt);
+
+ /* Disallow rectangles smaller than the minimal one. */
+ v4l2_rect_set_min_size(r, &crop_rect_min);
+ v4l2_rect_map_inside(r, &sink_rect);
+}
+
+static int vimc_sca_init_cfg(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg)
+{
+ struct v4l2_mbus_framefmt *mf;
+ struct v4l2_rect *r;
+ unsigned int i;
+
+ mf = v4l2_subdev_get_try_format(sd, cfg, 0);
+ *mf = sink_fmt_default;
+
+ r = v4l2_subdev_get_try_crop(sd, cfg, 0);
+ *r = crop_rect_default;
+
+ for (i = 1; i < sd->entity.num_pads; i++) {
+ mf = v4l2_subdev_get_try_format(sd, cfg, i);
+ *mf = sink_fmt_default;
+ mf->width = mf->width * sca_mult;
+ mf->height = mf->height * sca_mult;
+ }
+
+ return 0;
+}
+
+static int vimc_sca_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ u32 mbus_code = vimc_mbus_code_by_index(code->index);
+ const struct vimc_pix_map *vpix;
+
+ if (!mbus_code)
+ return -EINVAL;
+
+ vpix = vimc_pix_map_by_code(mbus_code);
+
+ /* We don't support bayer format */
+ if (!vpix || vpix->bayer)
+ return -EINVAL;
+
+ code->code = mbus_code;
+
+ return 0;
+}
+
+static int vimc_sca_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ const struct vimc_pix_map *vpix;
+
+ if (fse->index)
+ return -EINVAL;
+
+ /* Only accept code in the pix map table in non bayer format */
+ vpix = vimc_pix_map_by_code(fse->code);
+ if (!vpix || vpix->bayer)
+ return -EINVAL;
+
+ fse->min_width = VIMC_FRAME_MIN_WIDTH;
+ fse->min_height = VIMC_FRAME_MIN_HEIGHT;
+
+ if (VIMC_IS_SINK(fse->pad)) {
+ fse->max_width = VIMC_FRAME_MAX_WIDTH;
+ fse->max_height = VIMC_FRAME_MAX_HEIGHT;
+ } else {
+ fse->max_width = VIMC_FRAME_MAX_WIDTH * MAX_ZOOM;
+ fse->max_height = VIMC_FRAME_MAX_HEIGHT * MAX_ZOOM;
+ }
+
+ return 0;
+}
+
+static int vimc_sca_get_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *format)
+{
+ struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
+ struct v4l2_rect *crop_rect;
+
+ /* Get the current sink format */
+ if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
+ format->format = *v4l2_subdev_get_try_format(sd, cfg, 0);
+ crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
+ } else {
+ format->format = vsca->sink_fmt;
+ crop_rect = &vsca->crop_rect;
+ }
+
+ /* Scale the frame size for the source pad */
+ if (VIMC_IS_SRC(format->pad)) {
+ format->format.width = crop_rect->width * sca_mult;
+ format->format.height = crop_rect->height * sca_mult;
+ }
+
+ return 0;
+}
+
+static void vimc_sca_adjust_sink_fmt(struct v4l2_mbus_framefmt *fmt)
+{
+ const struct vimc_pix_map *vpix;
+
+ /* Only accept code in the pix map table in non bayer format */
+ vpix = vimc_pix_map_by_code(fmt->code);
+ if (!vpix || vpix->bayer)
+ fmt->code = sink_fmt_default.code;
+
+ fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
+ VIMC_FRAME_MAX_WIDTH) & ~1;
+ fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
+ VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+ if (fmt->field == V4L2_FIELD_ANY)
+ fmt->field = sink_fmt_default.field;
+
+ vimc_colorimetry_clamp(fmt);
+}
+
+static int vimc_sca_set_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *fmt)
+{
+ struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *sink_fmt;
+ struct v4l2_rect *crop_rect;
+
+ if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+ /* Do not change the format while stream is on */
+ if (vsca->src_frame)
+ return -EBUSY;
+
+ sink_fmt = &vsca->sink_fmt;
+ crop_rect = &vsca->crop_rect;
+ } else {
+ sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+ crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
+ }
+
+ /*
+ * Do not change the format of the source pad,
+ * it is propagated from the sink
+ */
+ if (VIMC_IS_SRC(fmt->pad)) {
+ fmt->format = *sink_fmt;
+ fmt->format.width = crop_rect->width * sca_mult;
+ fmt->format.height = crop_rect->height * sca_mult;
+ } else {
+ /* Set the new format in the sink pad */
+ vimc_sca_adjust_sink_fmt(&fmt->format);
+
+ dev_dbg(vsca->ved.dev, "%s: sink format update: "
+ "old:%dx%d (0x%x, %d, %d, %d, %d) "
+ "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsca->sd.name,
+ /* old */
+ sink_fmt->width, sink_fmt->height, sink_fmt->code,
+ sink_fmt->colorspace, sink_fmt->quantization,
+ sink_fmt->xfer_func, sink_fmt->ycbcr_enc,
+ /* new */
+ fmt->format.width, fmt->format.height, fmt->format.code,
+ fmt->format.colorspace, fmt->format.quantization,
+ fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+ *sink_fmt = fmt->format;
+
+ /* Do the crop, but respect the current bounds */
+ vimc_sca_adjust_sink_crop(crop_rect, sink_fmt);
+ }
+
+ return 0;
+}
+
+static int vimc_sca_get_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_selection *sel)
+{
+ struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *sink_fmt;
+ struct v4l2_rect *crop_rect;
+
+ if (VIMC_IS_SRC(sel->pad))
+ return -EINVAL;
+
+ if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+ sink_fmt = &vsca->sink_fmt;
+ crop_rect = &vsca->crop_rect;
+ } else {
+ sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+ crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
+ }
+
+ switch (sel->target) {
+ case V4L2_SEL_TGT_CROP:
+ sel->r = *crop_rect;
+ break;
+ case V4L2_SEL_TGT_CROP_BOUNDS:
+ sel->r = vimc_sca_get_crop_bound_sink(sink_fmt);
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static int vimc_sca_set_selection(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_selection *sel)
+{
+ struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *sink_fmt;
+ struct v4l2_rect *crop_rect;
+
+ if (VIMC_IS_SRC(sel->pad))
+ return -EINVAL;
+
+ if (sel->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+ /* Do not change the format while stream is on */
+ if (vsca->src_frame)
+ return -EBUSY;
+
+ crop_rect = &vsca->crop_rect;
+ sink_fmt = &vsca->sink_fmt;
+ } else {
+ crop_rect = v4l2_subdev_get_try_crop(sd, cfg, 0);
+ sink_fmt = v4l2_subdev_get_try_format(sd, cfg, 0);
+ }
+
+ switch (sel->target) {
+ case V4L2_SEL_TGT_CROP:
+ /* Do the crop, but respect the current bounds */
+ vimc_sca_adjust_sink_crop(&sel->r, sink_fmt);
+ *crop_rect = sel->r;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_sca_pad_ops = {
+ .init_cfg = vimc_sca_init_cfg,
+ .enum_mbus_code = vimc_sca_enum_mbus_code,
+ .enum_frame_size = vimc_sca_enum_frame_size,
+ .get_fmt = vimc_sca_get_fmt,
+ .set_fmt = vimc_sca_set_fmt,
+ .get_selection = vimc_sca_get_selection,
+ .set_selection = vimc_sca_set_selection,
+};
+
+static int vimc_sca_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct vimc_sca_device *vsca = v4l2_get_subdevdata(sd);
+
+ if (enable) {
+ const struct vimc_pix_map *vpix;
+ unsigned int frame_size;
+
+ if (vsca->src_frame)
+ return 0;
+
+ /* Save the bytes per pixel of the sink */
+ vpix = vimc_pix_map_by_code(vsca->sink_fmt.code);
+ vsca->bpp = vpix->bpp;
+
+ /* Calculate the width in bytes of the src frame */
+ vsca->src_line_size = vsca->crop_rect.width *
+ sca_mult * vsca->bpp;
+
+ /* Calculate the frame size of the source pad */
+ frame_size = vsca->src_line_size * vsca->crop_rect.height *
+ sca_mult;
+
+ /* Allocate the frame buffer. Use vmalloc to be able to
+ * allocate a large amount of memory
+ */
+ vsca->src_frame = vmalloc(frame_size);
+ if (!vsca->src_frame)
+ return -ENOMEM;
+
+ } else {
+ if (!vsca->src_frame)
+ return 0;
+
+ vfree(vsca->src_frame);
+ vsca->src_frame = NULL;
+ }
+
+ return 0;
+}
+
+static const struct v4l2_subdev_video_ops vimc_sca_video_ops = {
+ .s_stream = vimc_sca_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_sca_ops = {
+ .pad = &vimc_sca_pad_ops,
+ .video = &vimc_sca_video_ops,
+};
+
+static void vimc_sca_fill_pix(u8 *const ptr,
+ const u8 *const pixel,
+ const unsigned int bpp)
+{
+ unsigned int i;
+
+ /* copy the pixel to the pointer */
+ for (i = 0; i < bpp; i++)
+ ptr[i] = pixel[i];
+}
+
+static void vimc_sca_scale_pix(const struct vimc_sca_device *const vsca,
+ unsigned int lin, unsigned int col,
+ const u8 *const sink_frame)
+{
+ const struct v4l2_rect crop_rect = vsca->crop_rect;
+ unsigned int i, j, index;
+ const u8 *pixel;
+
+ /* Point to the pixel value in position (lin, col) in the sink frame */
+ index = VIMC_FRAME_INDEX(lin, col,
+ vsca->sink_fmt.width,
+ vsca->bpp);
+ pixel = &sink_frame[index];
+
+ dev_dbg(vsca->ved.dev,
+ "sca: %s: --- scale_pix sink pos %dx%d, index %d ---\n",
+ vsca->sd.name, lin, col, index);
+
+ /* point to the place we are going to put the first pixel
+ * in the scaled src frame
+ */
+ lin -= crop_rect.top;
+ col -= crop_rect.left;
+ index = VIMC_FRAME_INDEX(lin * sca_mult, col * sca_mult,
+ crop_rect.width * sca_mult, vsca->bpp);
+
+ dev_dbg(vsca->ved.dev, "sca: %s: scale_pix src pos %dx%d, index %d\n",
+ vsca->sd.name, lin * sca_mult, col * sca_mult, index);
+
+ /* Repeat this pixel mult times */
+ for (i = 0; i < sca_mult; i++) {
+ /* Iterate through each beginning of a
+ * pixel repetition in a line
+ */
+ for (j = 0; j < sca_mult * vsca->bpp; j += vsca->bpp) {
+ dev_dbg(vsca->ved.dev,
+ "sca: %s: sca: scale_pix src pos %d\n",
+ vsca->sd.name, index + j);
+
+ /* copy the pixel to the position index + j */
+ vimc_sca_fill_pix(&vsca->src_frame[index + j],
+ pixel, vsca->bpp);
+ }
+
+ /* move the index to the next line */
+ index += vsca->src_line_size;
+ }
+}
+
+static void vimc_sca_fill_src_frame(const struct vimc_sca_device *const vsca,
+ const u8 *const sink_frame)
+{
+ const struct v4l2_rect r = vsca->crop_rect;
+ unsigned int i, j;
+
+ /* Scale each pixel from the original sink frame */
+ /* TODO: implement scale down, only scale up is supported for now */
+ for (i = r.top; i < r.top + r.height; i++)
+ for (j = r.left; j < r.left + r.width; j++)
+ vimc_sca_scale_pix(vsca, i, j, sink_frame);
+}
+
+static void *vimc_sca_process_frame(struct vimc_ent_device *ved,
+ const void *sink_frame)
+{
+ struct vimc_sca_device *vsca = container_of(ved, struct vimc_sca_device,
+ ved);
+
+ /* If the stream in this node is not active, just return */
+ if (!vsca->src_frame)
+ return ERR_PTR(-EINVAL);
+
+ vimc_sca_fill_src_frame(vsca, sink_frame);
+
+ return vsca->src_frame;
+};
+
+static void vimc_sca_release(struct vimc_ent_device *ved)
+{
+ struct vimc_sca_device *vsca =
+ container_of(ved, struct vimc_sca_device, ved);
+
+ media_entity_cleanup(vsca->ved.ent);
+ kfree(vsca);
+}
+
+static struct vimc_ent_device *vimc_sca_add(struct vimc_device *vimc,
+ const char *vcfg_name)
+{
+ struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
+ struct vimc_sca_device *vsca;
+ int ret;
+
+ /* Allocate the vsca struct */
+ vsca = kzalloc(sizeof(*vsca), GFP_KERNEL);
+ if (!vsca)
+ return ERR_PTR(-ENOMEM);
+
+ /* Initialize ved and sd */
+ vsca->pads[0].flags = MEDIA_PAD_FL_SINK;
+ vsca->pads[1].flags = MEDIA_PAD_FL_SOURCE;
+
+ ret = vimc_ent_sd_register(&vsca->ved, &vsca->sd, v4l2_dev,
+ vcfg_name,
+ MEDIA_ENT_F_PROC_VIDEO_SCALER, 2,
+ vsca->pads, &vimc_sca_ops);
+ if (ret) {
+ kfree(vsca);
+ return ERR_PTR(ret);
+ }
+
+ vsca->ved.process_frame = vimc_sca_process_frame;
+ vsca->ved.dev = vimc->mdev.dev;
+
+ /* Initialize the frame format */
+ vsca->sink_fmt = sink_fmt_default;
+
+ /* Initialize the crop selection */
+ vsca->crop_rect = crop_rect_default;
+
+ return &vsca->ved;
+}
+
+struct vimc_ent_type vimc_sca_type = {
+ .add = vimc_sca_add,
+ .release = vimc_sca_release
+};
diff --git a/drivers/media/test-drivers/vimc/vimc-sensor.c b/drivers/media/test-drivers/vimc/vimc-sensor.c
new file mode 100644
index 0000000..ba5db5a
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/vimc-sensor.c
@@ -0,0 +1,452 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * vimc-sensor.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
+ */
+
+#include <linux/v4l2-mediabus.h>
+#include <linux/vmalloc.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-subdev.h>
+#include <media/tpg/v4l2-tpg.h>
+
+#include "vimc-common.h"
+
+enum vimc_sen_osd_mode {
+ VIMC_SEN_OSD_SHOW_ALL = 0,
+ VIMC_SEN_OSD_SHOW_COUNTERS = 1,
+ VIMC_SEN_OSD_SHOW_NONE = 2
+};
+
+struct vimc_sen_device {
+ struct vimc_ent_device ved;
+ struct v4l2_subdev sd;
+ struct tpg_data tpg;
+ u8 *frame;
+ enum vimc_sen_osd_mode osd_value;
+ u64 start_stream_ts;
+ /* The active format */
+ struct v4l2_mbus_framefmt mbus_format;
+ struct v4l2_ctrl_handler hdl;
+ struct media_pad pad;
+};
+
+static const struct v4l2_mbus_framefmt fmt_default = {
+ .width = 640,
+ .height = 480,
+ .code = MEDIA_BUS_FMT_RGB888_1X24,
+ .field = V4L2_FIELD_NONE,
+ .colorspace = V4L2_COLORSPACE_SRGB,
+};
+
+static int vimc_sen_init_cfg(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg)
+{
+ unsigned int i;
+
+ for (i = 0; i < sd->entity.num_pads; i++) {
+ struct v4l2_mbus_framefmt *mf;
+
+ mf = v4l2_subdev_get_try_format(sd, cfg, i);
+ *mf = fmt_default;
+ }
+
+ return 0;
+}
+
+static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_mbus_code_enum *code)
+{
+ u32 mbus_code = vimc_mbus_code_by_index(code->index);
+
+ if (!mbus_code)
+ return -EINVAL;
+
+ code->code = mbus_code;
+
+ return 0;
+}
+
+static int vimc_sen_enum_frame_size(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_frame_size_enum *fse)
+{
+ const struct vimc_pix_map *vpix;
+
+ if (fse->index)
+ return -EINVAL;
+
+ /* Only accept code in the pix map table */
+ vpix = vimc_pix_map_by_code(fse->code);
+ if (!vpix)
+ return -EINVAL;
+
+ fse->min_width = VIMC_FRAME_MIN_WIDTH;
+ fse->max_width = VIMC_FRAME_MAX_WIDTH;
+ fse->min_height = VIMC_FRAME_MIN_HEIGHT;
+ fse->max_height = VIMC_FRAME_MAX_HEIGHT;
+
+ return 0;
+}
+
+static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *fmt)
+{
+ struct vimc_sen_device *vsen =
+ container_of(sd, struct vimc_sen_device, sd);
+
+ fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
+ *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) :
+ vsen->mbus_format;
+
+ return 0;
+}
+
+static void vimc_sen_tpg_s_format(struct vimc_sen_device *vsen)
+{
+ const struct vimc_pix_map *vpix =
+ vimc_pix_map_by_code(vsen->mbus_format.code);
+
+ tpg_reset_source(&vsen->tpg, vsen->mbus_format.width,
+ vsen->mbus_format.height, vsen->mbus_format.field);
+ tpg_s_bytesperline(&vsen->tpg, 0, vsen->mbus_format.width * vpix->bpp);
+ tpg_s_buf_height(&vsen->tpg, vsen->mbus_format.height);
+ tpg_s_fourcc(&vsen->tpg, vpix->pixelformat);
+ /* TODO: add support for V4L2_FIELD_ALTERNATE */
+ tpg_s_field(&vsen->tpg, vsen->mbus_format.field, false);
+ tpg_s_colorspace(&vsen->tpg, vsen->mbus_format.colorspace);
+ tpg_s_ycbcr_enc(&vsen->tpg, vsen->mbus_format.ycbcr_enc);
+ tpg_s_quantization(&vsen->tpg, vsen->mbus_format.quantization);
+ tpg_s_xfer_func(&vsen->tpg, vsen->mbus_format.xfer_func);
+}
+
+static void vimc_sen_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
+{
+ const struct vimc_pix_map *vpix;
+
+ /* Only accept code in the pix map table */
+ vpix = vimc_pix_map_by_code(fmt->code);
+ if (!vpix)
+ fmt->code = fmt_default.code;
+
+ fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
+ VIMC_FRAME_MAX_WIDTH) & ~1;
+ fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
+ VIMC_FRAME_MAX_HEIGHT) & ~1;
+
+ /* TODO: add support for V4L2_FIELD_ALTERNATE */
+ if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
+ fmt->field = fmt_default.field;
+
+ vimc_colorimetry_clamp(fmt);
+}
+
+static int vimc_sen_set_fmt(struct v4l2_subdev *sd,
+ struct v4l2_subdev_pad_config *cfg,
+ struct v4l2_subdev_format *fmt)
+{
+ struct vimc_sen_device *vsen = v4l2_get_subdevdata(sd);
+ struct v4l2_mbus_framefmt *mf;
+
+ if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+ /* Do not change the format while stream is on */
+ if (vsen->frame)
+ return -EBUSY;
+
+ mf = &vsen->mbus_format;
+ } else {
+ mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
+ }
+
+ /* Set the new format */
+ vimc_sen_adjust_fmt(&fmt->format);
+
+ dev_dbg(vsen->ved.dev, "%s: format update: "
+ "old:%dx%d (0x%x, %d, %d, %d, %d) "
+ "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsen->sd.name,
+ /* old */
+ mf->width, mf->height, mf->code,
+ mf->colorspace, mf->quantization,
+ mf->xfer_func, mf->ycbcr_enc,
+ /* new */
+ fmt->format.width, fmt->format.height, fmt->format.code,
+ fmt->format.colorspace, fmt->format.quantization,
+ fmt->format.xfer_func, fmt->format.ycbcr_enc);
+
+ *mf = fmt->format;
+
+ return 0;
+}
+
+static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
+ .init_cfg = vimc_sen_init_cfg,
+ .enum_mbus_code = vimc_sen_enum_mbus_code,
+ .enum_frame_size = vimc_sen_enum_frame_size,
+ .get_fmt = vimc_sen_get_fmt,
+ .set_fmt = vimc_sen_set_fmt,
+};
+
+static void *vimc_sen_process_frame(struct vimc_ent_device *ved,
+ const void *sink_frame)
+{
+ struct vimc_sen_device *vsen = container_of(ved, struct vimc_sen_device,
+ ved);
+ const unsigned int line_height = 16;
+ u8 *basep[TPG_MAX_PLANES][2];
+ unsigned int line = 1;
+ char str[100];
+
+ tpg_fill_plane_buffer(&vsen->tpg, 0, 0, vsen->frame);
+ tpg_calc_text_basep(&vsen->tpg, basep, 0, vsen->frame);
+ switch (vsen->osd_value) {
+ case VIMC_SEN_OSD_SHOW_ALL: {
+ const char *order = tpg_g_color_order(&vsen->tpg);
+
+ tpg_gen_text(&vsen->tpg, basep, line++ * line_height,
+ 16, order);
+ snprintf(str, sizeof(str),
+ "brightness %3d, contrast %3d, saturation %3d, hue %d ",
+ vsen->tpg.brightness,
+ vsen->tpg.contrast,
+ vsen->tpg.saturation,
+ vsen->tpg.hue);
+ tpg_gen_text(&vsen->tpg, basep, line++ * line_height, 16, str);
+ snprintf(str, sizeof(str), "sensor size: %dx%d",
+ vsen->mbus_format.width,
+ vsen->mbus_format.height);
+ tpg_gen_text(&vsen->tpg, basep, line++ * line_height, 16, str);
+ fallthrough;
+ }
+ case VIMC_SEN_OSD_SHOW_COUNTERS: {
+ unsigned int ms;
+
+ ms = div_u64(ktime_get_ns() - vsen->start_stream_ts, 1000000);
+ snprintf(str, sizeof(str), "%02d:%02d:%02d:%03d",
+ (ms / (60 * 60 * 1000)) % 24,
+ (ms / (60 * 1000)) % 60,
+ (ms / 1000) % 60,
+ ms % 1000);
+ tpg_gen_text(&vsen->tpg, basep, line++ * line_height, 16, str);
+ break;
+ }
+ case VIMC_SEN_OSD_SHOW_NONE:
+ default:
+ break;
+ }
+
+ return vsen->frame;
+}
+
+static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
+{
+ struct vimc_sen_device *vsen =
+ container_of(sd, struct vimc_sen_device, sd);
+
+ if (enable) {
+ const struct vimc_pix_map *vpix;
+ unsigned int frame_size;
+
+ vsen->start_stream_ts = ktime_get_ns();
+
+ /* Calculate the frame size */
+ vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
+ frame_size = vsen->mbus_format.width * vpix->bpp *
+ vsen->mbus_format.height;
+
+ /*
+ * Allocate the frame buffer. Use vmalloc to be able to
+ * allocate a large amount of memory
+ */
+ vsen->frame = vmalloc(frame_size);
+ if (!vsen->frame)
+ return -ENOMEM;
+
+ /* configure the test pattern generator */
+ vimc_sen_tpg_s_format(vsen);
+
+ } else {
+
+ vfree(vsen->frame);
+ vsen->frame = NULL;
+ }
+
+ return 0;
+}
+
+static const struct v4l2_subdev_core_ops vimc_sen_core_ops = {
+ .log_status = v4l2_ctrl_subdev_log_status,
+ .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
+ .unsubscribe_event = v4l2_event_subdev_unsubscribe,
+};
+
+static const struct v4l2_subdev_video_ops vimc_sen_video_ops = {
+ .s_stream = vimc_sen_s_stream,
+};
+
+static const struct v4l2_subdev_ops vimc_sen_ops = {
+ .core = &vimc_sen_core_ops,
+ .pad = &vimc_sen_pad_ops,
+ .video = &vimc_sen_video_ops,
+};
+
+static int vimc_sen_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vimc_sen_device *vsen =
+ container_of(ctrl->handler, struct vimc_sen_device, hdl);
+
+ switch (ctrl->id) {
+ case VIMC_CID_TEST_PATTERN:
+ tpg_s_pattern(&vsen->tpg, ctrl->val);
+ break;
+ case V4L2_CID_HFLIP:
+ tpg_s_hflip(&vsen->tpg, ctrl->val);
+ break;
+ case V4L2_CID_VFLIP:
+ tpg_s_vflip(&vsen->tpg, ctrl->val);
+ break;
+ case V4L2_CID_BRIGHTNESS:
+ tpg_s_brightness(&vsen->tpg, ctrl->val);
+ break;
+ case V4L2_CID_CONTRAST:
+ tpg_s_contrast(&vsen->tpg, ctrl->val);
+ break;
+ case V4L2_CID_HUE:
+ tpg_s_hue(&vsen->tpg, ctrl->val);
+ break;
+ case V4L2_CID_SATURATION:
+ tpg_s_saturation(&vsen->tpg, ctrl->val);
+ break;
+ case VIMC_CID_OSD_TEXT_MODE:
+ vsen->osd_value = ctrl->val;
+ break;
+ default:
+ return -EINVAL;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vimc_sen_ctrl_ops = {
+ .s_ctrl = vimc_sen_s_ctrl,
+};
+
+static void vimc_sen_release(struct vimc_ent_device *ved)
+{
+ struct vimc_sen_device *vsen =
+ container_of(ved, struct vimc_sen_device, ved);
+
+ v4l2_ctrl_handler_free(&vsen->hdl);
+ tpg_free(&vsen->tpg);
+ media_entity_cleanup(vsen->ved.ent);
+ kfree(vsen);
+}
+
+/* Image Processing Controls */
+static const struct v4l2_ctrl_config vimc_sen_ctrl_class = {
+ .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
+ .id = VIMC_CID_VIMC_CLASS,
+ .name = "VIMC Controls",
+ .type = V4L2_CTRL_TYPE_CTRL_CLASS,
+};
+
+static const struct v4l2_ctrl_config vimc_sen_ctrl_test_pattern = {
+ .ops = &vimc_sen_ctrl_ops,
+ .id = VIMC_CID_TEST_PATTERN,
+ .name = "Test Pattern",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = TPG_PAT_NOISE,
+ .qmenu = tpg_pattern_strings,
+};
+
+static const char * const vimc_ctrl_osd_mode_strings[] = {
+ "All",
+ "Counters Only",
+ "None",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vimc_sen_ctrl_osd_mode = {
+ .ops = &vimc_sen_ctrl_ops,
+ .id = VIMC_CID_OSD_TEXT_MODE,
+ .name = "Show Information",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = ARRAY_SIZE(vimc_ctrl_osd_mode_strings) - 2,
+ .qmenu = vimc_ctrl_osd_mode_strings,
+};
+
+static struct vimc_ent_device *vimc_sen_add(struct vimc_device *vimc,
+ const char *vcfg_name)
+{
+ struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
+ struct vimc_sen_device *vsen;
+ int ret;
+
+ /* Allocate the vsen struct */
+ vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
+ if (!vsen)
+ return ERR_PTR(-ENOMEM);
+
+ v4l2_ctrl_handler_init(&vsen->hdl, 4);
+
+ v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_class, NULL);
+ v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_test_pattern, NULL);
+ v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_osd_mode, NULL);
+ v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
+ V4L2_CID_VFLIP, 0, 1, 1, 0);
+ v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
+ V4L2_CID_HFLIP, 0, 1, 1, 0);
+ v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
+ V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+ v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
+ V4L2_CID_CONTRAST, 0, 255, 1, 128);
+ v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
+ V4L2_CID_HUE, -128, 127, 1, 0);
+ v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
+ V4L2_CID_SATURATION, 0, 255, 1, 128);
+ vsen->sd.ctrl_handler = &vsen->hdl;
+ if (vsen->hdl.error) {
+ ret = vsen->hdl.error;
+ goto err_free_vsen;
+ }
+
+ /* Initialize the test pattern generator */
+ tpg_init(&vsen->tpg, vsen->mbus_format.width,
+ vsen->mbus_format.height);
+ ret = tpg_alloc(&vsen->tpg, VIMC_FRAME_MAX_WIDTH);
+ if (ret)
+ goto err_free_hdl;
+
+ /* Initialize ved and sd */
+ vsen->pad.flags = MEDIA_PAD_FL_SOURCE;
+ ret = vimc_ent_sd_register(&vsen->ved, &vsen->sd, v4l2_dev,
+ vcfg_name,
+ MEDIA_ENT_F_CAM_SENSOR, 1, &vsen->pad,
+ &vimc_sen_ops);
+ if (ret)
+ goto err_free_tpg;
+
+ vsen->ved.process_frame = vimc_sen_process_frame;
+ vsen->ved.dev = vimc->mdev.dev;
+
+ /* Initialize the frame format */
+ vsen->mbus_format = fmt_default;
+
+ return &vsen->ved;
+
+err_free_tpg:
+ tpg_free(&vsen->tpg);
+err_free_hdl:
+ v4l2_ctrl_handler_free(&vsen->hdl);
+err_free_vsen:
+ kfree(vsen);
+
+ return ERR_PTR(ret);
+}
+
+struct vimc_ent_type vimc_sen_type = {
+ .add = vimc_sen_add,
+ .release = vimc_sen_release
+};
diff --git a/drivers/media/test-drivers/vimc/vimc-streamer.c b/drivers/media/test-drivers/vimc/vimc-streamer.c
new file mode 100644
index 0000000..65feb3c
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/vimc-streamer.c
@@ -0,0 +1,238 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * vimc-streamer.c Virtual Media Controller Driver
+ *
+ * Copyright (C) 2018 Lucas A. M. Magalhães <lucmaga@gmail.com>
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/freezer.h>
+#include <linux/kthread.h>
+
+#include "vimc-streamer.h"
+
+/**
+ * vimc_get_source_entity - get the entity connected with the first sink pad
+ *
+ * @ent: reference media_entity
+ *
+ * Helper function that returns the media entity containing the source pad
+ * linked with the first sink pad from the given media entity pad list.
+ *
+ * Return: The source pad or NULL, if it wasn't found.
+ */
+static struct media_entity *vimc_get_source_entity(struct media_entity *ent)
+{
+ struct media_pad *pad;
+ int i;
+
+ for (i = 0; i < ent->num_pads; i++) {
+ if (ent->pads[i].flags & MEDIA_PAD_FL_SOURCE)
+ continue;
+ pad = media_entity_remote_pad(&ent->pads[i]);
+ return pad ? pad->entity : NULL;
+ }
+ return NULL;
+}
+
+/**
+ * vimc_streamer_pipeline_terminate - Disable stream in all ved in stream
+ *
+ * @stream: the pointer to the stream structure with the pipeline to be
+ * disabled.
+ *
+ * Calls s_stream to disable the stream in each entity of the pipeline
+ *
+ */
+static void vimc_streamer_pipeline_terminate(struct vimc_stream *stream)
+{
+ struct vimc_ent_device *ved;
+ struct v4l2_subdev *sd;
+
+ while (stream->pipe_size) {
+ stream->pipe_size--;
+ ved = stream->ved_pipeline[stream->pipe_size];
+ stream->ved_pipeline[stream->pipe_size] = NULL;
+
+ if (!is_media_entity_v4l2_subdev(ved->ent))
+ continue;
+
+ sd = media_entity_to_v4l2_subdev(ved->ent);
+ v4l2_subdev_call(sd, video, s_stream, 0);
+ }
+}
+
+/**
+ * vimc_streamer_pipeline_init - Initializes the stream structure
+ *
+ * @stream: the pointer to the stream structure to be initialized
+ * @ved: the pointer to the vimc entity initializing the stream
+ *
+ * Initializes the stream structure. Walks through the entity graph to
+ * construct the pipeline used later on the streamer thread.
+ * Calls vimc_streamer_s_stream() to enable stream in all entities of
+ * the pipeline.
+ *
+ * Return: 0 if success, error code otherwise.
+ */
+static int vimc_streamer_pipeline_init(struct vimc_stream *stream,
+ struct vimc_ent_device *ved)
+{
+ struct media_entity *entity;
+ struct video_device *vdev;
+ struct v4l2_subdev *sd;
+ int ret = 0;
+
+ stream->pipe_size = 0;
+ while (stream->pipe_size < VIMC_STREAMER_PIPELINE_MAX_SIZE) {
+ if (!ved) {
+ vimc_streamer_pipeline_terminate(stream);
+ return -EINVAL;
+ }
+ stream->ved_pipeline[stream->pipe_size++] = ved;
+
+ if (is_media_entity_v4l2_subdev(ved->ent)) {
+ sd = media_entity_to_v4l2_subdev(ved->ent);
+ ret = v4l2_subdev_call(sd, video, s_stream, 1);
+ if (ret && ret != -ENOIOCTLCMD) {
+ dev_err(ved->dev, "subdev_call error %s\n",
+ ved->ent->name);
+ vimc_streamer_pipeline_terminate(stream);
+ return ret;
+ }
+ }
+
+ entity = vimc_get_source_entity(ved->ent);
+ /* Check if the end of the pipeline was reached */
+ if (!entity) {
+ /* the first entity of the pipe should be source only */
+ if (!vimc_is_source(ved->ent)) {
+ dev_err(ved->dev,
+ "first entity in the pipe '%s' is not a source\n",
+ ved->ent->name);
+ vimc_streamer_pipeline_terminate(stream);
+ return -EPIPE;
+ }
+ return 0;
+ }
+
+ /* Get the next device in the pipeline */
+ if (is_media_entity_v4l2_subdev(entity)) {
+ sd = media_entity_to_v4l2_subdev(entity);
+ ved = v4l2_get_subdevdata(sd);
+ } else {
+ vdev = container_of(entity,
+ struct video_device,
+ entity);
+ ved = video_get_drvdata(vdev);
+ }
+ }
+
+ vimc_streamer_pipeline_terminate(stream);
+ return -EINVAL;
+}
+
+/**
+ * vimc_streamer_thread - Process frames through the pipeline
+ *
+ * @data: vimc_stream struct of the current stream
+ *
+ * From the source to the sink, gets a frame from each subdevice and send to
+ * the next one of the pipeline at a fixed framerate.
+ *
+ * Return:
+ * Always zero (created as ``int`` instead of ``void`` to comply with
+ * kthread API).
+ */
+static int vimc_streamer_thread(void *data)
+{
+ struct vimc_stream *stream = data;
+ u8 *frame = NULL;
+ int i;
+
+ set_freezable();
+
+ for (;;) {
+ try_to_freeze();
+ if (kthread_should_stop())
+ break;
+
+ for (i = stream->pipe_size - 1; i >= 0; i--) {
+ frame = stream->ved_pipeline[i]->process_frame(
+ stream->ved_pipeline[i], frame);
+ if (!frame || IS_ERR(frame))
+ break;
+ }
+ //wait for 60hz
+ set_current_state(TASK_UNINTERRUPTIBLE);
+ schedule_timeout(HZ / 60);
+ }
+
+ return 0;
+}
+
+/**
+ * vimc_streamer_s_stream - Start/stop the streaming on the media pipeline
+ *
+ * @stream: the pointer to the stream structure of the current stream
+ * @ved: pointer to the vimc entity of the entity of the stream
+ * @enable: flag to determine if stream should start/stop
+ *
+ * When starting, check if there is no ``stream->kthread`` allocated. This
+ * should indicate that a stream is already running. Then, it initializes the
+ * pipeline, creates and runs a kthread to consume buffers through the pipeline.
+ * When stopping, analogously check if there is a stream running, stop the
+ * thread and terminates the pipeline.
+ *
+ * Return: 0 if success, error code otherwise.
+ */
+int vimc_streamer_s_stream(struct vimc_stream *stream,
+ struct vimc_ent_device *ved,
+ int enable)
+{
+ int ret;
+
+ if (!stream || !ved)
+ return -EINVAL;
+
+ if (enable) {
+ if (stream->kthread)
+ return 0;
+
+ ret = vimc_streamer_pipeline_init(stream, ved);
+ if (ret)
+ return ret;
+
+ stream->kthread = kthread_run(vimc_streamer_thread, stream,
+ "vimc-streamer thread");
+
+ if (IS_ERR(stream->kthread)) {
+ ret = PTR_ERR(stream->kthread);
+ dev_err(ved->dev, "kthread_run failed with %d\n", ret);
+ vimc_streamer_pipeline_terminate(stream);
+ stream->kthread = NULL;
+ return ret;
+ }
+
+ } else {
+ if (!stream->kthread)
+ return 0;
+
+ ret = kthread_stop(stream->kthread);
+ /*
+ * kthread_stop returns -EINTR in cases when streamon was
+ * immediately followed by streamoff, and the thread didn't had
+ * a chance to run. Ignore errors to stop the stream in the
+ * pipeline.
+ */
+ if (ret)
+ dev_dbg(ved->dev, "kthread_stop returned '%d'\n", ret);
+
+ stream->kthread = NULL;
+
+ vimc_streamer_pipeline_terminate(stream);
+ }
+
+ return 0;
+}
diff --git a/drivers/media/test-drivers/vimc/vimc-streamer.h b/drivers/media/test-drivers/vimc/vimc-streamer.h
new file mode 100644
index 0000000..3bb6731
--- /dev/null
+++ b/drivers/media/test-drivers/vimc/vimc-streamer.h
@@ -0,0 +1,45 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * vimc-streamer.h Virtual Media Controller Driver
+ *
+ * Copyright (C) 2018 Lucas A. M. Magalhães <lucmaga@gmail.com>
+ *
+ */
+
+#ifndef _VIMC_STREAMER_H_
+#define _VIMC_STREAMER_H_
+
+#include <media/media-device.h>
+
+#include "vimc-common.h"
+
+#define VIMC_STREAMER_PIPELINE_MAX_SIZE 16
+
+/**
+ * struct vimc_stream - struct that represents a stream in the pipeline
+ *
+ * @pipe: the media pipeline object associated with this stream
+ * @ved_pipeline: array containing all the entities participating in the
+ * stream. The order is from a video device (usually a
+ * capture device) where stream_on was called, to the
+ * entity generating the first base image to be
+ * processed in the pipeline.
+ * @pipe_size: size of @ved_pipeline
+ * @kthread: thread that generates the frames of the stream.
+ *
+ * When the user call stream_on in a video device, struct vimc_stream is
+ * used to keep track of all entities and subdevices that generates and
+ * process frames for the stream.
+ */
+struct vimc_stream {
+ struct media_pipeline pipe;
+ struct vimc_ent_device *ved_pipeline[VIMC_STREAMER_PIPELINE_MAX_SIZE];
+ unsigned int pipe_size;
+ struct task_struct *kthread;
+};
+
+int vimc_streamer_s_stream(struct vimc_stream *stream,
+ struct vimc_ent_device *ved,
+ int enable);
+
+#endif //_VIMC_STREAMER_H_
diff --git a/drivers/media/test-drivers/vivid/Kconfig b/drivers/media/test-drivers/vivid/Kconfig
new file mode 100644
index 0000000..c3abde2
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/Kconfig
@@ -0,0 +1,43 @@
+# SPDX-License-Identifier: GPL-2.0-only
+config VIDEO_VIVID
+ tristate "Virtual Video Test Driver"
+ depends on VIDEO_DEV && VIDEO_V4L2 && !SPARC32 && !SPARC64 && FB
+ depends on HAS_DMA
+ select FONT_SUPPORT
+ select FONT_8x16
+ select FB_CFB_FILLRECT
+ select FB_CFB_COPYAREA
+ select FB_CFB_IMAGEBLIT
+ select VIDEOBUF2_VMALLOC
+ select VIDEOBUF2_DMA_CONTIG
+ select VIDEO_V4L2_TPG
+ select MEDIA_CONTROLLER
+ select MEDIA_CONTROLLER_REQUEST_API
+ help
+ Enables a virtual video driver. This driver emulates a webcam,
+ TV, S-Video and HDMI capture hardware, including VBI support for
+ the SDTV inputs. Also video output, VBI output, radio receivers,
+ transmitters and software defined radio capture is emulated.
+
+ It is highly configurable and is ideal for testing applications.
+ Error injection is supported to test rare errors that are hard
+ to reproduce in real hardware.
+
+ Say Y here if you want to test video apps or debug V4L devices.
+ When in doubt, say N.
+
+config VIDEO_VIVID_CEC
+ bool "Enable CEC emulation support"
+ depends on VIDEO_VIVID
+ select CEC_CORE
+ help
+ When selected the vivid module will emulate the optional
+ HDMI CEC feature.
+
+config VIDEO_VIVID_MAX_DEVS
+ int "Maximum number of devices"
+ depends on VIDEO_VIVID
+ default "64"
+ help
+ This allows you to specify the maximum number of devices supported
+ by the vivid driver.
diff --git a/drivers/media/test-drivers/vivid/Makefile b/drivers/media/test-drivers/vivid/Makefile
new file mode 100644
index 0000000..b12ad01
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/Makefile
@@ -0,0 +1,12 @@
+# SPDX-License-Identifier: GPL-2.0
+vivid-objs := vivid-core.o vivid-ctrls.o vivid-vid-common.o vivid-vbi-gen.o \
+ vivid-vid-cap.o vivid-vid-out.o vivid-kthread-cap.o vivid-kthread-out.o \
+ vivid-radio-rx.o vivid-radio-tx.o vivid-radio-common.o \
+ vivid-rds-gen.o vivid-sdr-cap.o vivid-vbi-cap.o vivid-vbi-out.o \
+ vivid-osd.o vivid-meta-cap.o vivid-meta-out.o \
+ vivid-kthread-touch.o vivid-touch-cap.o
+ifeq ($(CONFIG_VIDEO_VIVID_CEC),y)
+ vivid-objs += vivid-cec.o
+endif
+
+obj-$(CONFIG_VIDEO_VIVID) += vivid.o
diff --git a/drivers/media/test-drivers/vivid/vivid-cec.c b/drivers/media/test-drivers/vivid/vivid-cec.c
new file mode 100644
index 0000000..4d2413e
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-cec.c
@@ -0,0 +1,286 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-cec.c - A Virtual Video Test Driver, cec emulation
+ *
+ * Copyright 2016 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <media/cec.h>
+
+#include "vivid-core.h"
+#include "vivid-cec.h"
+
+#define CEC_TIM_START_BIT_TOTAL 4500
+#define CEC_TIM_START_BIT_LOW 3700
+#define CEC_TIM_START_BIT_HIGH 800
+#define CEC_TIM_DATA_BIT_TOTAL 2400
+#define CEC_TIM_DATA_BIT_0_LOW 1500
+#define CEC_TIM_DATA_BIT_0_HIGH 900
+#define CEC_TIM_DATA_BIT_1_LOW 600
+#define CEC_TIM_DATA_BIT_1_HIGH 1800
+
+void vivid_cec_bus_free_work(struct vivid_dev *dev)
+{
+ spin_lock(&dev->cec_slock);
+ while (!list_empty(&dev->cec_work_list)) {
+ struct vivid_cec_work *cw =
+ list_first_entry(&dev->cec_work_list,
+ struct vivid_cec_work, list);
+
+ spin_unlock(&dev->cec_slock);
+ cancel_delayed_work_sync(&cw->work);
+ spin_lock(&dev->cec_slock);
+ list_del(&cw->list);
+ cec_transmit_attempt_done(cw->adap, CEC_TX_STATUS_LOW_DRIVE);
+ kfree(cw);
+ }
+ spin_unlock(&dev->cec_slock);
+}
+
+static bool vivid_cec_find_dest_adap(struct vivid_dev *dev,
+ struct cec_adapter *adap, u8 dest)
+{
+ unsigned int i;
+
+ if (dest >= 0xf)
+ return false;
+
+ if (adap != dev->cec_rx_adap && dev->cec_rx_adap &&
+ dev->cec_rx_adap->is_configured &&
+ cec_has_log_addr(dev->cec_rx_adap, dest))
+ return true;
+
+ for (i = 0; i < MAX_OUTPUTS && dev->cec_tx_adap[i]; i++) {
+ if (adap == dev->cec_tx_adap[i])
+ continue;
+ if (!dev->cec_tx_adap[i]->is_configured)
+ continue;
+ if (cec_has_log_addr(dev->cec_tx_adap[i], dest))
+ return true;
+ }
+ return false;
+}
+
+static void vivid_cec_pin_adap_events(struct cec_adapter *adap, ktime_t ts,
+ const struct cec_msg *msg, bool nacked)
+{
+ unsigned int len = nacked ? 1 : msg->len;
+ unsigned int i;
+ bool bit;
+
+ if (adap == NULL)
+ return;
+
+ /*
+ * Suffix ULL on constant 10 makes the expression
+ * CEC_TIM_START_BIT_TOTAL + 10ULL * len * CEC_TIM_DATA_BIT_TOTAL
+ * to be evaluated using 64-bit unsigned arithmetic (u64), which
+ * is what ktime_sub_us expects as second argument.
+ */
+ ts = ktime_sub_us(ts, CEC_TIM_START_BIT_TOTAL +
+ 10ULL * len * CEC_TIM_DATA_BIT_TOTAL);
+ cec_queue_pin_cec_event(adap, false, false, ts);
+ ts = ktime_add_us(ts, CEC_TIM_START_BIT_LOW);
+ cec_queue_pin_cec_event(adap, true, false, ts);
+ ts = ktime_add_us(ts, CEC_TIM_START_BIT_HIGH);
+
+ for (i = 0; i < 10 * len; i++) {
+ switch (i % 10) {
+ case 0 ... 7:
+ bit = msg->msg[i / 10] & (0x80 >> (i % 10));
+ break;
+ case 8: /* EOM */
+ bit = i / 10 == msg->len - 1;
+ break;
+ case 9: /* ACK */
+ bit = cec_msg_is_broadcast(msg) ^ nacked;
+ break;
+ }
+ cec_queue_pin_cec_event(adap, false, false, ts);
+ if (bit)
+ ts = ktime_add_us(ts, CEC_TIM_DATA_BIT_1_LOW);
+ else
+ ts = ktime_add_us(ts, CEC_TIM_DATA_BIT_0_LOW);
+ cec_queue_pin_cec_event(adap, true, false, ts);
+ if (bit)
+ ts = ktime_add_us(ts, CEC_TIM_DATA_BIT_1_HIGH);
+ else
+ ts = ktime_add_us(ts, CEC_TIM_DATA_BIT_0_HIGH);
+ }
+}
+
+static void vivid_cec_pin_events(struct vivid_dev *dev,
+ const struct cec_msg *msg, bool nacked)
+{
+ ktime_t ts = ktime_get();
+ unsigned int i;
+
+ vivid_cec_pin_adap_events(dev->cec_rx_adap, ts, msg, nacked);
+ for (i = 0; i < MAX_OUTPUTS; i++)
+ vivid_cec_pin_adap_events(dev->cec_tx_adap[i], ts, msg, nacked);
+}
+
+static void vivid_cec_xfer_done_worker(struct work_struct *work)
+{
+ struct vivid_cec_work *cw =
+ container_of(work, struct vivid_cec_work, work.work);
+ struct vivid_dev *dev = cw->dev;
+ struct cec_adapter *adap = cw->adap;
+ u8 dest = cec_msg_destination(&cw->msg);
+ bool valid_dest;
+ unsigned int i;
+
+ valid_dest = cec_msg_is_broadcast(&cw->msg);
+ if (!valid_dest)
+ valid_dest = vivid_cec_find_dest_adap(dev, adap, dest);
+
+ cw->tx_status = valid_dest ? CEC_TX_STATUS_OK : CEC_TX_STATUS_NACK;
+ spin_lock(&dev->cec_slock);
+ dev->cec_xfer_time_jiffies = 0;
+ dev->cec_xfer_start_jiffies = 0;
+ list_del(&cw->list);
+ spin_unlock(&dev->cec_slock);
+ vivid_cec_pin_events(dev, &cw->msg, !valid_dest);
+ cec_transmit_attempt_done(cw->adap, cw->tx_status);
+
+ /* Broadcast message */
+ if (adap != dev->cec_rx_adap)
+ cec_received_msg(dev->cec_rx_adap, &cw->msg);
+ for (i = 0; i < MAX_OUTPUTS && dev->cec_tx_adap[i]; i++)
+ if (adap != dev->cec_tx_adap[i])
+ cec_received_msg(dev->cec_tx_adap[i], &cw->msg);
+ kfree(cw);
+}
+
+static void vivid_cec_xfer_try_worker(struct work_struct *work)
+{
+ struct vivid_cec_work *cw =
+ container_of(work, struct vivid_cec_work, work.work);
+ struct vivid_dev *dev = cw->dev;
+
+ spin_lock(&dev->cec_slock);
+ if (dev->cec_xfer_time_jiffies) {
+ list_del(&cw->list);
+ spin_unlock(&dev->cec_slock);
+ cec_transmit_attempt_done(cw->adap, CEC_TX_STATUS_ARB_LOST);
+ kfree(cw);
+ } else {
+ INIT_DELAYED_WORK(&cw->work, vivid_cec_xfer_done_worker);
+ dev->cec_xfer_start_jiffies = jiffies;
+ dev->cec_xfer_time_jiffies = usecs_to_jiffies(cw->usecs);
+ spin_unlock(&dev->cec_slock);
+ schedule_delayed_work(&cw->work, dev->cec_xfer_time_jiffies);
+ }
+}
+
+static int vivid_cec_adap_enable(struct cec_adapter *adap, bool enable)
+{
+ adap->cec_pin_is_high = true;
+ return 0;
+}
+
+static int vivid_cec_adap_log_addr(struct cec_adapter *adap, u8 log_addr)
+{
+ return 0;
+}
+
+/*
+ * One data bit takes 2400 us, each byte needs 10 bits so that's 24000 us
+ * per byte.
+ */
+#define USECS_PER_BYTE 24000
+
+static int vivid_cec_adap_transmit(struct cec_adapter *adap, u8 attempts,
+ u32 signal_free_time, struct cec_msg *msg)
+{
+ struct vivid_dev *dev = cec_get_drvdata(adap);
+ struct vivid_cec_work *cw = kzalloc(sizeof(*cw), GFP_KERNEL);
+ long delta_jiffies = 0;
+
+ if (cw == NULL)
+ return -ENOMEM;
+ cw->dev = dev;
+ cw->adap = adap;
+ cw->usecs = CEC_FREE_TIME_TO_USEC(signal_free_time) +
+ msg->len * USECS_PER_BYTE;
+ cw->msg = *msg;
+
+ spin_lock(&dev->cec_slock);
+ list_add(&cw->list, &dev->cec_work_list);
+ if (dev->cec_xfer_time_jiffies == 0) {
+ INIT_DELAYED_WORK(&cw->work, vivid_cec_xfer_done_worker);
+ dev->cec_xfer_start_jiffies = jiffies;
+ dev->cec_xfer_time_jiffies = usecs_to_jiffies(cw->usecs);
+ delta_jiffies = dev->cec_xfer_time_jiffies;
+ } else {
+ INIT_DELAYED_WORK(&cw->work, vivid_cec_xfer_try_worker);
+ delta_jiffies = dev->cec_xfer_start_jiffies +
+ dev->cec_xfer_time_jiffies - jiffies;
+ }
+ spin_unlock(&dev->cec_slock);
+ schedule_delayed_work(&cw->work, delta_jiffies < 0 ? 0 : delta_jiffies);
+ return 0;
+}
+
+static int vivid_received(struct cec_adapter *adap, struct cec_msg *msg)
+{
+ struct vivid_dev *dev = cec_get_drvdata(adap);
+ struct cec_msg reply;
+ u8 dest = cec_msg_destination(msg);
+ u8 disp_ctl;
+ char osd[14];
+
+ if (cec_msg_is_broadcast(msg))
+ dest = adap->log_addrs.log_addr[0];
+ cec_msg_init(&reply, dest, cec_msg_initiator(msg));
+
+ switch (cec_msg_opcode(msg)) {
+ case CEC_MSG_SET_OSD_STRING:
+ if (!cec_is_sink(adap))
+ return -ENOMSG;
+ cec_ops_set_osd_string(msg, &disp_ctl, osd);
+ switch (disp_ctl) {
+ case CEC_OP_DISP_CTL_DEFAULT:
+ strscpy(dev->osd, osd, sizeof(dev->osd));
+ dev->osd_jiffies = jiffies;
+ break;
+ case CEC_OP_DISP_CTL_UNTIL_CLEARED:
+ strscpy(dev->osd, osd, sizeof(dev->osd));
+ dev->osd_jiffies = 0;
+ break;
+ case CEC_OP_DISP_CTL_CLEAR:
+ dev->osd[0] = 0;
+ dev->osd_jiffies = 0;
+ break;
+ default:
+ cec_msg_feature_abort(&reply, cec_msg_opcode(msg),
+ CEC_OP_ABORT_INVALID_OP);
+ cec_transmit_msg(adap, &reply, false);
+ break;
+ }
+ break;
+ default:
+ return -ENOMSG;
+ }
+ return 0;
+}
+
+static const struct cec_adap_ops vivid_cec_adap_ops = {
+ .adap_enable = vivid_cec_adap_enable,
+ .adap_log_addr = vivid_cec_adap_log_addr,
+ .adap_transmit = vivid_cec_adap_transmit,
+ .received = vivid_received,
+};
+
+struct cec_adapter *vivid_cec_alloc_adap(struct vivid_dev *dev,
+ unsigned int idx,
+ bool is_source)
+{
+ u32 caps = CEC_CAP_DEFAULTS | CEC_CAP_MONITOR_ALL | CEC_CAP_MONITOR_PIN;
+ char name[32];
+
+ snprintf(name, sizeof(name), "vivid-%03d-vid-%s%d",
+ dev->inst, is_source ? "out" : "cap", idx);
+ return cec_allocate_adapter(&vivid_cec_adap_ops, dev,
+ name, caps, 1);
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-cec.h b/drivers/media/test-drivers/vivid/vivid-cec.h
new file mode 100644
index 0000000..7524ed4
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-cec.h
@@ -0,0 +1,20 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-cec.h - A Virtual Video Test Driver, cec emulation
+ *
+ * Copyright 2016 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifdef CONFIG_VIDEO_VIVID_CEC
+struct cec_adapter *vivid_cec_alloc_adap(struct vivid_dev *dev,
+ unsigned int idx,
+ bool is_source);
+void vivid_cec_bus_free_work(struct vivid_dev *dev);
+
+#else
+
+static inline void vivid_cec_bus_free_work(struct vivid_dev *dev)
+{
+}
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-core.c b/drivers/media/test-drivers/vivid/vivid-core.c
new file mode 100644
index 0000000..1e356dc
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-core.c
@@ -0,0 +1,2083 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-core.c - A Virtual Video Test Driver, core initialization
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-rx.h"
+#include "vivid-radio-tx.h"
+#include "vivid-sdr-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-out.h"
+#include "vivid-osd.h"
+#include "vivid-cec.h"
+#include "vivid-ctrls.h"
+#include "vivid-meta-cap.h"
+#include "vivid-meta-out.h"
+#include "vivid-touch-cap.h"
+
+#define VIVID_MODULE_NAME "vivid"
+
+/* The maximum number of vivid devices */
+#define VIVID_MAX_DEVS CONFIG_VIDEO_VIVID_MAX_DEVS
+
+MODULE_DESCRIPTION("Virtual Video Test Driver");
+MODULE_AUTHOR("Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+static unsigned n_devs = 1;
+module_param(n_devs, uint, 0444);
+MODULE_PARM_DESC(n_devs, " number of driver instances to create");
+
+static int vid_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vid_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vid_cap_nr, " videoX start number, -1 is autodetect");
+
+static int vid_out_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vid_out_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vid_out_nr, " videoX start number, -1 is autodetect");
+
+static int vbi_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vbi_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vbi_cap_nr, " vbiX start number, -1 is autodetect");
+
+static int vbi_out_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vbi_out_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vbi_out_nr, " vbiX start number, -1 is autodetect");
+
+static int sdr_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(sdr_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(sdr_cap_nr, " swradioX start number, -1 is autodetect");
+
+static int radio_rx_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(radio_rx_nr, int, NULL, 0444);
+MODULE_PARM_DESC(radio_rx_nr, " radioX start number, -1 is autodetect");
+
+static int radio_tx_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(radio_tx_nr, int, NULL, 0444);
+MODULE_PARM_DESC(radio_tx_nr, " radioX start number, -1 is autodetect");
+
+static int meta_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(meta_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(meta_cap_nr, " videoX start number, -1 is autodetect");
+
+static int meta_out_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(meta_out_nr, int, NULL, 0444);
+MODULE_PARM_DESC(meta_out_nr, " videoX start number, -1 is autodetect");
+
+static int touch_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(touch_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(touch_cap_nr, " v4l-touchX start number, -1 is autodetect");
+
+static int ccs_cap_mode[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(ccs_cap_mode, int, NULL, 0444);
+MODULE_PARM_DESC(ccs_cap_mode, " capture crop/compose/scale mode:\n"
+ "\t\t bit 0=crop, 1=compose, 2=scale,\n"
+ "\t\t -1=user-controlled (default)");
+
+static int ccs_out_mode[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(ccs_out_mode, int, NULL, 0444);
+MODULE_PARM_DESC(ccs_out_mode, " output crop/compose/scale mode:\n"
+ "\t\t bit 0=crop, 1=compose, 2=scale,\n"
+ "\t\t -1=user-controlled (default)");
+
+static unsigned multiplanar[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 1 };
+module_param_array(multiplanar, uint, NULL, 0444);
+MODULE_PARM_DESC(multiplanar, " 1 (default) creates a single planar device, 2 creates a multiplanar device.");
+
+/*
+ * Default: video + vbi-cap (raw and sliced) + radio rx + radio tx + sdr +
+ * vbi-out + vid-out + meta-cap
+ */
+static unsigned int node_types[VIVID_MAX_DEVS] = {
+ [0 ... (VIVID_MAX_DEVS - 1)] = 0xe1d3d
+};
+module_param_array(node_types, uint, NULL, 0444);
+MODULE_PARM_DESC(node_types, " node types, default is 0xe1d3d. Bitmask with the following meaning:\n"
+ "\t\t bit 0: Video Capture node\n"
+ "\t\t bit 2-3: VBI Capture node: 0 = none, 1 = raw vbi, 2 = sliced vbi, 3 = both\n"
+ "\t\t bit 4: Radio Receiver node\n"
+ "\t\t bit 5: Software Defined Radio Receiver node\n"
+ "\t\t bit 8: Video Output node\n"
+ "\t\t bit 10-11: VBI Output node: 0 = none, 1 = raw vbi, 2 = sliced vbi, 3 = both\n"
+ "\t\t bit 12: Radio Transmitter node\n"
+ "\t\t bit 16: Framebuffer for testing overlays\n"
+ "\t\t bit 17: Metadata Capture node\n"
+ "\t\t bit 18: Metadata Output node\n"
+ "\t\t bit 19: Touch Capture node\n");
+
+/* Default: 4 inputs */
+static unsigned num_inputs[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 4 };
+module_param_array(num_inputs, uint, NULL, 0444);
+MODULE_PARM_DESC(num_inputs, " number of inputs, default is 4");
+
+/* Default: input 0 = WEBCAM, 1 = TV, 2 = SVID, 3 = HDMI */
+static unsigned input_types[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 0xe4 };
+module_param_array(input_types, uint, NULL, 0444);
+MODULE_PARM_DESC(input_types, " input types, default is 0xe4. Two bits per input,\n"
+ "\t\t bits 0-1 == input 0, bits 31-30 == input 15.\n"
+ "\t\t Type 0 == webcam, 1 == TV, 2 == S-Video, 3 == HDMI");
+
+/* Default: 2 outputs */
+static unsigned num_outputs[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 2 };
+module_param_array(num_outputs, uint, NULL, 0444);
+MODULE_PARM_DESC(num_outputs, " number of outputs, default is 2");
+
+/* Default: output 0 = SVID, 1 = HDMI */
+static unsigned output_types[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 2 };
+module_param_array(output_types, uint, NULL, 0444);
+MODULE_PARM_DESC(output_types, " output types, default is 0x02. One bit per output,\n"
+ "\t\t bit 0 == output 0, bit 15 == output 15.\n"
+ "\t\t Type 0 == S-Video, 1 == HDMI");
+
+unsigned vivid_debug;
+module_param(vivid_debug, uint, 0644);
+MODULE_PARM_DESC(vivid_debug, " activates debug info");
+
+static bool no_error_inj;
+module_param(no_error_inj, bool, 0444);
+MODULE_PARM_DESC(no_error_inj, " if set disable the error injecting controls");
+
+static unsigned int allocators[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 0 };
+module_param_array(allocators, uint, NULL, 0444);
+MODULE_PARM_DESC(allocators, " memory allocator selection, default is 0.\n"
+ "\t\t 0 == vmalloc\n"
+ "\t\t 1 == dma-contig");
+
+static unsigned int cache_hints[VIVID_MAX_DEVS] = {
+ [0 ... (VIVID_MAX_DEVS - 1)] = 0
+};
+module_param_array(cache_hints, uint, NULL, 0444);
+MODULE_PARM_DESC(cache_hints, " user-space cache hints, default is 0.\n"
+ "\t\t 0 == forbid\n"
+ "\t\t 1 == allow");
+
+static struct vivid_dev *vivid_devs[VIVID_MAX_DEVS];
+
+const struct v4l2_rect vivid_min_rect = {
+ 0, 0, MIN_WIDTH, MIN_HEIGHT
+};
+
+const struct v4l2_rect vivid_max_rect = {
+ 0, 0, MAX_WIDTH * MAX_ZOOM, MAX_HEIGHT * MAX_ZOOM
+};
+
+static const u8 vivid_hdmi_edid[256] = {
+ 0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+ 0x31, 0xd8, 0x34, 0x12, 0x00, 0x00, 0x00, 0x00,
+ 0x22, 0x1a, 0x01, 0x03, 0x80, 0x60, 0x36, 0x78,
+ 0x0f, 0xee, 0x91, 0xa3, 0x54, 0x4c, 0x99, 0x26,
+ 0x0f, 0x50, 0x54, 0x2f, 0xcf, 0x00, 0x31, 0x59,
+ 0x45, 0x59, 0x81, 0x80, 0x81, 0x40, 0x90, 0x40,
+ 0x95, 0x00, 0xa9, 0x40, 0xb3, 0x00, 0x08, 0xe8,
+ 0x00, 0x30, 0xf2, 0x70, 0x5a, 0x80, 0xb0, 0x58,
+ 0x8a, 0x00, 0xc0, 0x1c, 0x32, 0x00, 0x00, 0x1e,
+ 0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x55, 0x18,
+ 0x87, 0x3c, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
+ 0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00, 0x76,
+ 0x69, 0x76, 0x69, 0x64, 0x0a, 0x20, 0x20, 0x20,
+ 0x20, 0x20, 0x20, 0x20, 0x00, 0x00, 0x00, 0x10,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x7b,
+
+ 0x02, 0x03, 0x3f, 0xf1, 0x51, 0x61, 0x60, 0x5f,
+ 0x5e, 0x5d, 0x10, 0x1f, 0x04, 0x13, 0x22, 0x21,
+ 0x20, 0x05, 0x14, 0x02, 0x11, 0x01, 0x23, 0x09,
+ 0x07, 0x07, 0x83, 0x01, 0x00, 0x00, 0x6d, 0x03,
+ 0x0c, 0x00, 0x10, 0x00, 0x00, 0x3c, 0x21, 0x00,
+ 0x60, 0x01, 0x02, 0x03, 0x67, 0xd8, 0x5d, 0xc4,
+ 0x01, 0x78, 0x00, 0x00, 0xe2, 0x00, 0xca, 0xe3,
+ 0x05, 0x00, 0x00, 0xe3, 0x06, 0x01, 0x00, 0x4d,
+ 0xd0, 0x00, 0xa0, 0xf0, 0x70, 0x3e, 0x80, 0x30,
+ 0x20, 0x35, 0x00, 0xc0, 0x1c, 0x32, 0x00, 0x00,
+ 0x1e, 0x1a, 0x36, 0x80, 0xa0, 0x70, 0x38, 0x1f,
+ 0x40, 0x30, 0x20, 0x35, 0x00, 0xc0, 0x1c, 0x32,
+ 0x00, 0x00, 0x1a, 0x1a, 0x1d, 0x00, 0x80, 0x51,
+ 0xd0, 0x1c, 0x20, 0x40, 0x80, 0x35, 0x00, 0xc0,
+ 0x1c, 0x32, 0x00, 0x00, 0x1c, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x82,
+};
+
+static int vidioc_querycap(struct file *file, void *priv,
+ struct v4l2_capability *cap)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ strscpy(cap->driver, "vivid", sizeof(cap->driver));
+ strscpy(cap->card, "vivid", sizeof(cap->card));
+ snprintf(cap->bus_info, sizeof(cap->bus_info),
+ "platform:%s", dev->v4l2_dev.name);
+
+ cap->capabilities = dev->vid_cap_caps | dev->vid_out_caps |
+ dev->vbi_cap_caps | dev->vbi_out_caps |
+ dev->radio_rx_caps | dev->radio_tx_caps |
+ dev->sdr_cap_caps | dev->meta_cap_caps |
+ dev->meta_out_caps | dev->touch_cap_caps |
+ V4L2_CAP_DEVICE_CAPS;
+ return 0;
+}
+
+static int vidioc_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2_hw_freq_seek *a)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_RADIO)
+ return vivid_radio_rx_s_hw_freq_seek(file, fh, a);
+ return -ENOTTY;
+}
+
+static int vidioc_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_RADIO)
+ return vivid_radio_rx_enum_freq_bands(file, fh, band);
+ if (vdev->vfl_type == VFL_TYPE_SDR)
+ return vivid_sdr_enum_freq_bands(file, fh, band);
+ return -ENOTTY;
+}
+
+static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_RADIO)
+ return vivid_radio_rx_g_tuner(file, fh, vt);
+ if (vdev->vfl_type == VFL_TYPE_SDR)
+ return vivid_sdr_g_tuner(file, fh, vt);
+ return vivid_video_g_tuner(file, fh, vt);
+}
+
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_RADIO)
+ return vivid_radio_rx_s_tuner(file, fh, vt);
+ if (vdev->vfl_type == VFL_TYPE_SDR)
+ return vivid_sdr_s_tuner(file, fh, vt);
+ return vivid_video_s_tuner(file, fh, vt);
+}
+
+static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_RADIO)
+ return vivid_radio_g_frequency(file,
+ vdev->vfl_dir == VFL_DIR_RX ?
+ &dev->radio_rx_freq : &dev->radio_tx_freq, vf);
+ if (vdev->vfl_type == VFL_TYPE_SDR)
+ return vivid_sdr_g_frequency(file, fh, vf);
+ return vivid_video_g_frequency(file, fh, vf);
+}
+
+static int vidioc_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_RADIO)
+ return vivid_radio_s_frequency(file,
+ vdev->vfl_dir == VFL_DIR_RX ?
+ &dev->radio_rx_freq : &dev->radio_tx_freq, vf);
+ if (vdev->vfl_type == VFL_TYPE_SDR)
+ return vivid_sdr_s_frequency(file, fh, vf);
+ return vivid_video_s_frequency(file, fh, vf);
+}
+
+static int vidioc_overlay(struct file *file, void *fh, unsigned i)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_overlay(file, fh, i);
+ return vivid_vid_out_overlay(file, fh, i);
+}
+
+static int vidioc_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *a)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_g_fbuf(file, fh, a);
+ return vivid_vid_out_g_fbuf(file, fh, a);
+}
+
+static int vidioc_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *a)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_s_fbuf(file, fh, a);
+ return vivid_vid_out_s_fbuf(file, fh, a);
+}
+
+static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id id)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_s_std(file, fh, id);
+ return vivid_vid_out_s_std(file, fh, id);
+}
+
+static int vidioc_s_dv_timings(struct file *file, void *fh, struct v4l2_dv_timings *timings)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_s_dv_timings(file, fh, timings);
+ return vivid_vid_out_s_dv_timings(file, fh, timings);
+}
+
+static int vidioc_g_pixelaspect(struct file *file, void *fh,
+ int type, struct v4l2_fract *f)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_g_pixelaspect(file, fh, type, f);
+ return vivid_vid_out_g_pixelaspect(file, fh, type, f);
+}
+
+static int vidioc_g_selection(struct file *file, void *fh,
+ struct v4l2_selection *sel)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_g_selection(file, fh, sel);
+ return vivid_vid_out_g_selection(file, fh, sel);
+}
+
+static int vidioc_s_selection(struct file *file, void *fh,
+ struct v4l2_selection *sel)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_s_selection(file, fh, sel);
+ return vivid_vid_out_s_selection(file, fh, sel);
+}
+
+static int vidioc_g_parm(struct file *file, void *fh,
+ struct v4l2_streamparm *parm)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_g_parm_tch(file, fh, parm);
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_g_parm(file, fh, parm);
+ return vivid_vid_out_g_parm(file, fh, parm);
+}
+
+static int vidioc_s_parm(struct file *file, void *fh,
+ struct v4l2_streamparm *parm)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_vid_cap_s_parm(file, fh, parm);
+ return -ENOTTY;
+}
+
+static int vidioc_log_status(struct file *file, void *fh)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+
+ v4l2_ctrl_log_status(file, fh);
+ if (vdev->vfl_dir == VFL_DIR_RX && vdev->vfl_type == VFL_TYPE_VIDEO)
+ tpg_log_status(&dev->tpg);
+ return 0;
+}
+
+static ssize_t vivid_radio_read(struct file *file, char __user *buf,
+ size_t size, loff_t *offset)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_TX)
+ return -EINVAL;
+ return vivid_radio_rx_read(file, buf, size, offset);
+}
+
+static ssize_t vivid_radio_write(struct file *file, const char __user *buf,
+ size_t size, loff_t *offset)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return -EINVAL;
+ return vivid_radio_tx_write(file, buf, size, offset);
+}
+
+static __poll_t vivid_radio_poll(struct file *file, struct poll_table_struct *wait)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX)
+ return vivid_radio_rx_poll(file, wait);
+ return vivid_radio_tx_poll(file, wait);
+}
+
+static int vivid_enum_input(struct file *file, void *priv,
+ struct v4l2_input *inp)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_enum_input_tch(file, priv, inp);
+ return vidioc_enum_input(file, priv, inp);
+}
+
+static int vivid_g_input(struct file *file, void *priv, unsigned int *i)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_g_input_tch(file, priv, i);
+ return vidioc_g_input(file, priv, i);
+}
+
+static int vivid_s_input(struct file *file, void *priv, unsigned int i)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_s_input_tch(file, priv, i);
+ return vidioc_s_input(file, priv, i);
+}
+
+static int vivid_enum_fmt_cap(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_enum_fmt_tch(file, priv, f);
+ return vivid_enum_fmt_vid(file, priv, f);
+}
+
+static int vivid_g_fmt_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_g_fmt_tch(file, priv, f);
+ return vidioc_g_fmt_vid_cap(file, priv, f);
+}
+
+static int vivid_try_fmt_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_g_fmt_tch(file, priv, f);
+ return vidioc_try_fmt_vid_cap(file, priv, f);
+}
+
+static int vivid_s_fmt_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_g_fmt_tch(file, priv, f);
+ return vidioc_s_fmt_vid_cap(file, priv, f);
+}
+
+static int vivid_g_fmt_cap_mplane(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_g_fmt_tch_mplane(file, priv, f);
+ return vidioc_g_fmt_vid_cap_mplane(file, priv, f);
+}
+
+static int vivid_try_fmt_cap_mplane(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_g_fmt_tch_mplane(file, priv, f);
+ return vidioc_try_fmt_vid_cap_mplane(file, priv, f);
+}
+
+static int vivid_s_fmt_cap_mplane(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_type == VFL_TYPE_TOUCH)
+ return vivid_g_fmt_tch_mplane(file, priv, f);
+ return vidioc_s_fmt_vid_cap_mplane(file, priv, f);
+}
+
+static bool vivid_is_in_use(struct video_device *vdev)
+{
+ unsigned long flags;
+ bool res;
+
+ spin_lock_irqsave(&vdev->fh_lock, flags);
+ res = !list_empty(&vdev->fh_list);
+ spin_unlock_irqrestore(&vdev->fh_lock, flags);
+ return res;
+}
+
+static bool vivid_is_last_user(struct vivid_dev *dev)
+{
+ unsigned uses = vivid_is_in_use(&dev->vid_cap_dev) +
+ vivid_is_in_use(&dev->vid_out_dev) +
+ vivid_is_in_use(&dev->vbi_cap_dev) +
+ vivid_is_in_use(&dev->vbi_out_dev) +
+ vivid_is_in_use(&dev->sdr_cap_dev) +
+ vivid_is_in_use(&dev->radio_rx_dev) +
+ vivid_is_in_use(&dev->radio_tx_dev) +
+ vivid_is_in_use(&dev->meta_cap_dev) +
+ vivid_is_in_use(&dev->meta_out_dev) +
+ vivid_is_in_use(&dev->touch_cap_dev);
+
+ return uses == 1;
+}
+
+static int vivid_fop_release(struct file *file)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+
+ mutex_lock(&dev->mutex);
+ if (!no_error_inj && v4l2_fh_is_singular_file(file) &&
+ !video_is_registered(vdev) && vivid_is_last_user(dev)) {
+ /*
+ * I am the last user of this driver, and a disconnect
+ * was forced (since this video_device is unregistered),
+ * so re-register all video_device's again.
+ */
+ v4l2_info(&dev->v4l2_dev, "reconnect\n");
+ set_bit(V4L2_FL_REGISTERED, &dev->vid_cap_dev.flags);
+ set_bit(V4L2_FL_REGISTERED, &dev->vid_out_dev.flags);
+ set_bit(V4L2_FL_REGISTERED, &dev->vbi_cap_dev.flags);
+ set_bit(V4L2_FL_REGISTERED, &dev->vbi_out_dev.flags);
+ set_bit(V4L2_FL_REGISTERED, &dev->sdr_cap_dev.flags);
+ set_bit(V4L2_FL_REGISTERED, &dev->radio_rx_dev.flags);
+ set_bit(V4L2_FL_REGISTERED, &dev->radio_tx_dev.flags);
+ set_bit(V4L2_FL_REGISTERED, &dev->meta_cap_dev.flags);
+ set_bit(V4L2_FL_REGISTERED, &dev->meta_out_dev.flags);
+ set_bit(V4L2_FL_REGISTERED, &dev->touch_cap_dev.flags);
+ }
+ mutex_unlock(&dev->mutex);
+ if (file->private_data == dev->overlay_cap_owner)
+ dev->overlay_cap_owner = NULL;
+ if (file->private_data == dev->radio_rx_rds_owner) {
+ dev->radio_rx_rds_last_block = 0;
+ dev->radio_rx_rds_owner = NULL;
+ }
+ if (file->private_data == dev->radio_tx_rds_owner) {
+ dev->radio_tx_rds_last_block = 0;
+ dev->radio_tx_rds_owner = NULL;
+ }
+ if (vdev->queue)
+ return vb2_fop_release(file);
+ return v4l2_fh_release(file);
+}
+
+static const struct v4l2_file_operations vivid_fops = {
+ .owner = THIS_MODULE,
+ .open = v4l2_fh_open,
+ .release = vivid_fop_release,
+ .read = vb2_fop_read,
+ .write = vb2_fop_write,
+ .poll = vb2_fop_poll,
+ .unlocked_ioctl = video_ioctl2,
+ .mmap = vb2_fop_mmap,
+};
+
+static const struct v4l2_file_operations vivid_radio_fops = {
+ .owner = THIS_MODULE,
+ .open = v4l2_fh_open,
+ .release = vivid_fop_release,
+ .read = vivid_radio_read,
+ .write = vivid_radio_write,
+ .poll = vivid_radio_poll,
+ .unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops vivid_ioctl_ops = {
+ .vidioc_querycap = vidioc_querycap,
+
+ .vidioc_enum_fmt_vid_cap = vivid_enum_fmt_cap,
+ .vidioc_g_fmt_vid_cap = vivid_g_fmt_cap,
+ .vidioc_try_fmt_vid_cap = vivid_try_fmt_cap,
+ .vidioc_s_fmt_vid_cap = vivid_s_fmt_cap,
+ .vidioc_g_fmt_vid_cap_mplane = vivid_g_fmt_cap_mplane,
+ .vidioc_try_fmt_vid_cap_mplane = vivid_try_fmt_cap_mplane,
+ .vidioc_s_fmt_vid_cap_mplane = vivid_s_fmt_cap_mplane,
+
+ .vidioc_enum_fmt_vid_out = vivid_enum_fmt_vid,
+ .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out,
+ .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
+ .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out,
+ .vidioc_g_fmt_vid_out_mplane = vidioc_g_fmt_vid_out_mplane,
+ .vidioc_try_fmt_vid_out_mplane = vidioc_try_fmt_vid_out_mplane,
+ .vidioc_s_fmt_vid_out_mplane = vidioc_s_fmt_vid_out_mplane,
+
+ .vidioc_g_selection = vidioc_g_selection,
+ .vidioc_s_selection = vidioc_s_selection,
+ .vidioc_g_pixelaspect = vidioc_g_pixelaspect,
+
+ .vidioc_g_fmt_vbi_cap = vidioc_g_fmt_vbi_cap,
+ .vidioc_try_fmt_vbi_cap = vidioc_g_fmt_vbi_cap,
+ .vidioc_s_fmt_vbi_cap = vidioc_s_fmt_vbi_cap,
+
+ .vidioc_g_fmt_sliced_vbi_cap = vidioc_g_fmt_sliced_vbi_cap,
+ .vidioc_try_fmt_sliced_vbi_cap = vidioc_try_fmt_sliced_vbi_cap,
+ .vidioc_s_fmt_sliced_vbi_cap = vidioc_s_fmt_sliced_vbi_cap,
+ .vidioc_g_sliced_vbi_cap = vidioc_g_sliced_vbi_cap,
+
+ .vidioc_g_fmt_vbi_out = vidioc_g_fmt_vbi_out,
+ .vidioc_try_fmt_vbi_out = vidioc_g_fmt_vbi_out,
+ .vidioc_s_fmt_vbi_out = vidioc_s_fmt_vbi_out,
+
+ .vidioc_g_fmt_sliced_vbi_out = vidioc_g_fmt_sliced_vbi_out,
+ .vidioc_try_fmt_sliced_vbi_out = vidioc_try_fmt_sliced_vbi_out,
+ .vidioc_s_fmt_sliced_vbi_out = vidioc_s_fmt_sliced_vbi_out,
+
+ .vidioc_enum_fmt_sdr_cap = vidioc_enum_fmt_sdr_cap,
+ .vidioc_g_fmt_sdr_cap = vidioc_g_fmt_sdr_cap,
+ .vidioc_try_fmt_sdr_cap = vidioc_try_fmt_sdr_cap,
+ .vidioc_s_fmt_sdr_cap = vidioc_s_fmt_sdr_cap,
+
+ .vidioc_overlay = vidioc_overlay,
+ .vidioc_enum_framesizes = vidioc_enum_framesizes,
+ .vidioc_enum_frameintervals = vidioc_enum_frameintervals,
+ .vidioc_g_parm = vidioc_g_parm,
+ .vidioc_s_parm = vidioc_s_parm,
+
+ .vidioc_enum_fmt_vid_overlay = vidioc_enum_fmt_vid_overlay,
+ .vidioc_g_fmt_vid_overlay = vidioc_g_fmt_vid_overlay,
+ .vidioc_try_fmt_vid_overlay = vidioc_try_fmt_vid_overlay,
+ .vidioc_s_fmt_vid_overlay = vidioc_s_fmt_vid_overlay,
+ .vidioc_g_fmt_vid_out_overlay = vidioc_g_fmt_vid_out_overlay,
+ .vidioc_try_fmt_vid_out_overlay = vidioc_try_fmt_vid_out_overlay,
+ .vidioc_s_fmt_vid_out_overlay = vidioc_s_fmt_vid_out_overlay,
+ .vidioc_g_fbuf = vidioc_g_fbuf,
+ .vidioc_s_fbuf = vidioc_s_fbuf,
+
+ .vidioc_reqbufs = vb2_ioctl_reqbufs,
+ .vidioc_create_bufs = vb2_ioctl_create_bufs,
+ .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
+ .vidioc_querybuf = vb2_ioctl_querybuf,
+ .vidioc_qbuf = vb2_ioctl_qbuf,
+ .vidioc_dqbuf = vb2_ioctl_dqbuf,
+ .vidioc_expbuf = vb2_ioctl_expbuf,
+ .vidioc_streamon = vb2_ioctl_streamon,
+ .vidioc_streamoff = vb2_ioctl_streamoff,
+
+ .vidioc_enum_input = vivid_enum_input,
+ .vidioc_g_input = vivid_g_input,
+ .vidioc_s_input = vivid_s_input,
+ .vidioc_s_audio = vidioc_s_audio,
+ .vidioc_g_audio = vidioc_g_audio,
+ .vidioc_enumaudio = vidioc_enumaudio,
+ .vidioc_s_frequency = vidioc_s_frequency,
+ .vidioc_g_frequency = vidioc_g_frequency,
+ .vidioc_s_tuner = vidioc_s_tuner,
+ .vidioc_g_tuner = vidioc_g_tuner,
+ .vidioc_s_modulator = vidioc_s_modulator,
+ .vidioc_g_modulator = vidioc_g_modulator,
+ .vidioc_s_hw_freq_seek = vidioc_s_hw_freq_seek,
+ .vidioc_enum_freq_bands = vidioc_enum_freq_bands,
+
+ .vidioc_enum_output = vidioc_enum_output,
+ .vidioc_g_output = vidioc_g_output,
+ .vidioc_s_output = vidioc_s_output,
+ .vidioc_s_audout = vidioc_s_audout,
+ .vidioc_g_audout = vidioc_g_audout,
+ .vidioc_enumaudout = vidioc_enumaudout,
+
+ .vidioc_querystd = vidioc_querystd,
+ .vidioc_g_std = vidioc_g_std,
+ .vidioc_s_std = vidioc_s_std,
+ .vidioc_s_dv_timings = vidioc_s_dv_timings,
+ .vidioc_g_dv_timings = vidioc_g_dv_timings,
+ .vidioc_query_dv_timings = vidioc_query_dv_timings,
+ .vidioc_enum_dv_timings = vidioc_enum_dv_timings,
+ .vidioc_dv_timings_cap = vidioc_dv_timings_cap,
+ .vidioc_g_edid = vidioc_g_edid,
+ .vidioc_s_edid = vidioc_s_edid,
+
+ .vidioc_log_status = vidioc_log_status,
+ .vidioc_subscribe_event = vidioc_subscribe_event,
+ .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+
+ .vidioc_enum_fmt_meta_cap = vidioc_enum_fmt_meta_cap,
+ .vidioc_g_fmt_meta_cap = vidioc_g_fmt_meta_cap,
+ .vidioc_s_fmt_meta_cap = vidioc_g_fmt_meta_cap,
+ .vidioc_try_fmt_meta_cap = vidioc_g_fmt_meta_cap,
+
+ .vidioc_enum_fmt_meta_out = vidioc_enum_fmt_meta_out,
+ .vidioc_g_fmt_meta_out = vidioc_g_fmt_meta_out,
+ .vidioc_s_fmt_meta_out = vidioc_g_fmt_meta_out,
+ .vidioc_try_fmt_meta_out = vidioc_g_fmt_meta_out,
+};
+
+/* -----------------------------------------------------------------
+ Initialization and module stuff
+ ------------------------------------------------------------------*/
+
+static void vivid_dev_release(struct v4l2_device *v4l2_dev)
+{
+ struct vivid_dev *dev = container_of(v4l2_dev, struct vivid_dev, v4l2_dev);
+
+ vivid_free_controls(dev);
+ v4l2_device_unregister(&dev->v4l2_dev);
+#ifdef CONFIG_MEDIA_CONTROLLER
+ media_device_cleanup(&dev->mdev);
+#endif
+ vfree(dev->scaled_line);
+ vfree(dev->blended_line);
+ vfree(dev->edid);
+ vfree(dev->bitmap_cap);
+ vfree(dev->bitmap_out);
+ tpg_free(&dev->tpg);
+ kfree(dev->query_dv_timings_qmenu);
+ kfree(dev->query_dv_timings_qmenu_strings);
+ kfree(dev);
+}
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+static int vivid_req_validate(struct media_request *req)
+{
+ struct vivid_dev *dev = container_of(req->mdev, struct vivid_dev, mdev);
+
+ if (dev->req_validate_error) {
+ dev->req_validate_error = false;
+ return -EINVAL;
+ }
+ return vb2_request_validate(req);
+}
+
+static const struct media_device_ops vivid_media_ops = {
+ .req_validate = vivid_req_validate,
+ .req_queue = vb2_request_queue,
+};
+#endif
+
+static int vivid_create_queue(struct vivid_dev *dev,
+ struct vb2_queue *q,
+ u32 buf_type,
+ unsigned int min_buffers_needed,
+ const struct vb2_ops *ops)
+{
+ if (buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE && dev->multiplanar)
+ buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+ else if (buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT && dev->multiplanar)
+ buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+ else if (buf_type == V4L2_BUF_TYPE_VBI_CAPTURE && !dev->has_raw_vbi_cap)
+ buf_type = V4L2_BUF_TYPE_SLICED_VBI_CAPTURE;
+ else if (buf_type == V4L2_BUF_TYPE_VBI_OUTPUT && !dev->has_raw_vbi_out)
+ buf_type = V4L2_BUF_TYPE_SLICED_VBI_OUTPUT;
+
+ q->type = buf_type;
+ q->io_modes = VB2_MMAP | VB2_DMABUF;
+ q->io_modes |= V4L2_TYPE_IS_OUTPUT(buf_type) ? VB2_WRITE : VB2_READ;
+ if (allocators[dev->inst] != 1)
+ q->io_modes |= VB2_USERPTR;
+ q->drv_priv = dev;
+ q->buf_struct_size = sizeof(struct vivid_buffer);
+ q->ops = ops;
+ q->mem_ops = allocators[dev->inst] == 1 ? &vb2_dma_contig_memops :
+ &vb2_vmalloc_memops;
+ q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+ q->min_buffers_needed = min_buffers_needed;
+ q->lock = &dev->mutex;
+ q->dev = dev->v4l2_dev.dev;
+ q->supports_requests = true;
+ q->allow_cache_hints = (cache_hints[dev->inst] == 1);
+
+ return vb2_queue_init(q);
+}
+
+static int vivid_detect_feature_set(struct vivid_dev *dev, int inst,
+ unsigned node_type,
+ bool *has_tuner,
+ bool *has_modulator,
+ int *ccs_cap,
+ int *ccs_out,
+ unsigned in_type_counter[4],
+ unsigned out_type_counter[4])
+{
+ int i;
+
+ /* do we use single- or multi-planar? */
+ dev->multiplanar = multiplanar[inst] > 1;
+ v4l2_info(&dev->v4l2_dev, "using %splanar format API\n",
+ dev->multiplanar ? "multi" : "single ");
+
+ /* how many inputs do we have and of what type? */
+ dev->num_inputs = num_inputs[inst];
+ if (dev->num_inputs < 1)
+ dev->num_inputs = 1;
+ if (dev->num_inputs >= MAX_INPUTS)
+ dev->num_inputs = MAX_INPUTS;
+ for (i = 0; i < dev->num_inputs; i++) {
+ dev->input_type[i] = (input_types[inst] >> (i * 2)) & 0x3;
+ dev->input_name_counter[i] = in_type_counter[dev->input_type[i]]++;
+ }
+ dev->has_audio_inputs = in_type_counter[TV] && in_type_counter[SVID];
+ if (in_type_counter[HDMI] == 16) {
+ /* The CEC physical address only allows for max 15 inputs */
+ in_type_counter[HDMI]--;
+ dev->num_inputs--;
+ }
+ dev->num_hdmi_inputs = in_type_counter[HDMI];
+
+ /* how many outputs do we have and of what type? */
+ dev->num_outputs = num_outputs[inst];
+ if (dev->num_outputs < 1)
+ dev->num_outputs = 1;
+ if (dev->num_outputs >= MAX_OUTPUTS)
+ dev->num_outputs = MAX_OUTPUTS;
+ for (i = 0; i < dev->num_outputs; i++) {
+ dev->output_type[i] = ((output_types[inst] >> i) & 1) ? HDMI : SVID;
+ dev->output_name_counter[i] = out_type_counter[dev->output_type[i]]++;
+ dev->display_present[i] = true;
+ }
+ dev->has_audio_outputs = out_type_counter[SVID];
+ if (out_type_counter[HDMI] == 16) {
+ /*
+ * The CEC physical address only allows for max 15 inputs,
+ * so outputs are also limited to 15 to allow for easy
+ * CEC output to input mapping.
+ */
+ out_type_counter[HDMI]--;
+ dev->num_outputs--;
+ }
+ dev->num_hdmi_outputs = out_type_counter[HDMI];
+
+ /* do we create a video capture device? */
+ dev->has_vid_cap = node_type & 0x0001;
+
+ /* do we create a vbi capture device? */
+ if (in_type_counter[TV] || in_type_counter[SVID]) {
+ dev->has_raw_vbi_cap = node_type & 0x0004;
+ dev->has_sliced_vbi_cap = node_type & 0x0008;
+ dev->has_vbi_cap = dev->has_raw_vbi_cap | dev->has_sliced_vbi_cap;
+ }
+
+ /* do we create a meta capture device */
+ dev->has_meta_cap = node_type & 0x20000;
+
+ /* sanity checks */
+ if ((in_type_counter[WEBCAM] || in_type_counter[HDMI]) &&
+ !dev->has_vid_cap && !dev->has_meta_cap) {
+ v4l2_warn(&dev->v4l2_dev,
+ "Webcam or HDMI input without video or metadata nodes\n");
+ return -EINVAL;
+ }
+ if ((in_type_counter[TV] || in_type_counter[SVID]) &&
+ !dev->has_vid_cap && !dev->has_vbi_cap && !dev->has_meta_cap) {
+ v4l2_warn(&dev->v4l2_dev,
+ "TV or S-Video input without video, VBI or metadata nodes\n");
+ return -EINVAL;
+ }
+
+ /* do we create a video output device? */
+ dev->has_vid_out = node_type & 0x0100;
+
+ /* do we create a vbi output device? */
+ if (out_type_counter[SVID]) {
+ dev->has_raw_vbi_out = node_type & 0x0400;
+ dev->has_sliced_vbi_out = node_type & 0x0800;
+ dev->has_vbi_out = dev->has_raw_vbi_out | dev->has_sliced_vbi_out;
+ }
+
+ /* do we create a metadata output device */
+ dev->has_meta_out = node_type & 0x40000;
+
+ /* sanity checks */
+ if (out_type_counter[SVID] &&
+ !dev->has_vid_out && !dev->has_vbi_out && !dev->has_meta_out) {
+ v4l2_warn(&dev->v4l2_dev,
+ "S-Video output without video, VBI or metadata nodes\n");
+ return -EINVAL;
+ }
+ if (out_type_counter[HDMI] && !dev->has_vid_out && !dev->has_meta_out) {
+ v4l2_warn(&dev->v4l2_dev,
+ "HDMI output without video or metadata nodes\n");
+ return -EINVAL;
+ }
+
+ /* do we create a radio receiver device? */
+ dev->has_radio_rx = node_type & 0x0010;
+
+ /* do we create a radio transmitter device? */
+ dev->has_radio_tx = node_type & 0x1000;
+
+ /* do we create a software defined radio capture device? */
+ dev->has_sdr_cap = node_type & 0x0020;
+
+ /* do we have a TV tuner? */
+ dev->has_tv_tuner = in_type_counter[TV];
+
+ /* do we have a tuner? */
+ *has_tuner = ((dev->has_vid_cap || dev->has_vbi_cap) && in_type_counter[TV]) ||
+ dev->has_radio_rx || dev->has_sdr_cap;
+
+ /* do we have a modulator? */
+ *has_modulator = dev->has_radio_tx;
+
+ if (dev->has_vid_cap)
+ /* do we have a framebuffer for overlay testing? */
+ dev->has_fb = node_type & 0x10000;
+
+ /* can we do crop/compose/scaling while capturing? */
+ if (no_error_inj && *ccs_cap == -1)
+ *ccs_cap = 7;
+
+ /* if ccs_cap == -1, then the user can select it using controls */
+ if (*ccs_cap != -1) {
+ dev->has_crop_cap = *ccs_cap & 1;
+ dev->has_compose_cap = *ccs_cap & 2;
+ dev->has_scaler_cap = *ccs_cap & 4;
+ v4l2_info(&dev->v4l2_dev, "Capture Crop: %c Compose: %c Scaler: %c\n",
+ dev->has_crop_cap ? 'Y' : 'N',
+ dev->has_compose_cap ? 'Y' : 'N',
+ dev->has_scaler_cap ? 'Y' : 'N');
+ }
+
+ /* can we do crop/compose/scaling with video output? */
+ if (no_error_inj && *ccs_out == -1)
+ *ccs_out = 7;
+
+ /* if ccs_out == -1, then the user can select it using controls */
+ if (*ccs_out != -1) {
+ dev->has_crop_out = *ccs_out & 1;
+ dev->has_compose_out = *ccs_out & 2;
+ dev->has_scaler_out = *ccs_out & 4;
+ v4l2_info(&dev->v4l2_dev, "Output Crop: %c Compose: %c Scaler: %c\n",
+ dev->has_crop_out ? 'Y' : 'N',
+ dev->has_compose_out ? 'Y' : 'N',
+ dev->has_scaler_out ? 'Y' : 'N');
+ }
+
+ /* do we create a touch capture device */
+ dev->has_touch_cap = node_type & 0x80000;
+
+ return 0;
+}
+
+static void vivid_set_capabilities(struct vivid_dev *dev)
+{
+ if (dev->has_vid_cap) {
+ /* set up the capabilities of the video capture device */
+ dev->vid_cap_caps = dev->multiplanar ?
+ V4L2_CAP_VIDEO_CAPTURE_MPLANE :
+ V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY;
+ dev->vid_cap_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+ if (dev->has_audio_inputs)
+ dev->vid_cap_caps |= V4L2_CAP_AUDIO;
+ if (dev->has_tv_tuner)
+ dev->vid_cap_caps |= V4L2_CAP_TUNER;
+ }
+ if (dev->has_vid_out) {
+ /* set up the capabilities of the video output device */
+ dev->vid_out_caps = dev->multiplanar ?
+ V4L2_CAP_VIDEO_OUTPUT_MPLANE :
+ V4L2_CAP_VIDEO_OUTPUT;
+ if (dev->has_fb)
+ dev->vid_out_caps |= V4L2_CAP_VIDEO_OUTPUT_OVERLAY;
+ dev->vid_out_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+ if (dev->has_audio_outputs)
+ dev->vid_out_caps |= V4L2_CAP_AUDIO;
+ }
+ if (dev->has_vbi_cap) {
+ /* set up the capabilities of the vbi capture device */
+ dev->vbi_cap_caps = (dev->has_raw_vbi_cap ? V4L2_CAP_VBI_CAPTURE : 0) |
+ (dev->has_sliced_vbi_cap ? V4L2_CAP_SLICED_VBI_CAPTURE : 0);
+ dev->vbi_cap_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+ if (dev->has_audio_inputs)
+ dev->vbi_cap_caps |= V4L2_CAP_AUDIO;
+ if (dev->has_tv_tuner)
+ dev->vbi_cap_caps |= V4L2_CAP_TUNER;
+ }
+ if (dev->has_vbi_out) {
+ /* set up the capabilities of the vbi output device */
+ dev->vbi_out_caps = (dev->has_raw_vbi_out ? V4L2_CAP_VBI_OUTPUT : 0) |
+ (dev->has_sliced_vbi_out ? V4L2_CAP_SLICED_VBI_OUTPUT : 0);
+ dev->vbi_out_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+ if (dev->has_audio_outputs)
+ dev->vbi_out_caps |= V4L2_CAP_AUDIO;
+ }
+ if (dev->has_sdr_cap) {
+ /* set up the capabilities of the sdr capture device */
+ dev->sdr_cap_caps = V4L2_CAP_SDR_CAPTURE | V4L2_CAP_TUNER;
+ dev->sdr_cap_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+ }
+ /* set up the capabilities of the radio receiver device */
+ if (dev->has_radio_rx)
+ dev->radio_rx_caps = V4L2_CAP_RADIO | V4L2_CAP_RDS_CAPTURE |
+ V4L2_CAP_HW_FREQ_SEEK | V4L2_CAP_TUNER |
+ V4L2_CAP_READWRITE;
+ /* set up the capabilities of the radio transmitter device */
+ if (dev->has_radio_tx)
+ dev->radio_tx_caps = V4L2_CAP_RDS_OUTPUT | V4L2_CAP_MODULATOR |
+ V4L2_CAP_READWRITE;
+
+ /* set up the capabilities of meta capture device */
+ if (dev->has_meta_cap) {
+ dev->meta_cap_caps = V4L2_CAP_META_CAPTURE |
+ V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+ if (dev->has_audio_inputs)
+ dev->meta_cap_caps |= V4L2_CAP_AUDIO;
+ if (dev->has_tv_tuner)
+ dev->meta_cap_caps |= V4L2_CAP_TUNER;
+ }
+ /* set up the capabilities of meta output device */
+ if (dev->has_meta_out) {
+ dev->meta_out_caps = V4L2_CAP_META_OUTPUT |
+ V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+ if (dev->has_audio_outputs)
+ dev->meta_out_caps |= V4L2_CAP_AUDIO;
+ }
+ /* set up the capabilities of the touch capture device */
+ if (dev->has_touch_cap) {
+ dev->touch_cap_caps = V4L2_CAP_TOUCH | V4L2_CAP_STREAMING |
+ V4L2_CAP_READWRITE;
+ dev->touch_cap_caps |= dev->multiplanar ?
+ V4L2_CAP_VIDEO_CAPTURE_MPLANE : V4L2_CAP_VIDEO_CAPTURE;
+ }
+}
+
+static void vivid_disable_unused_ioctls(struct vivid_dev *dev,
+ bool has_tuner,
+ bool has_modulator,
+ unsigned in_type_counter[4],
+ unsigned out_type_counter[4])
+{
+ /* disable invalid ioctls based on the feature set */
+ if (!dev->has_audio_inputs) {
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_AUDIO);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_AUDIO);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_ENUMAUDIO);
+ v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_AUDIO);
+ v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_G_AUDIO);
+ v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_ENUMAUDIO);
+ v4l2_disable_ioctl(&dev->meta_cap_dev, VIDIOC_S_AUDIO);
+ v4l2_disable_ioctl(&dev->meta_cap_dev, VIDIOC_G_AUDIO);
+ v4l2_disable_ioctl(&dev->meta_cap_dev, VIDIOC_ENUMAUDIO);
+ }
+ if (!dev->has_audio_outputs) {
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_AUDOUT);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_AUDOUT);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUMAUDOUT);
+ v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_S_AUDOUT);
+ v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_G_AUDOUT);
+ v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_ENUMAUDOUT);
+ v4l2_disable_ioctl(&dev->meta_out_dev, VIDIOC_S_AUDOUT);
+ v4l2_disable_ioctl(&dev->meta_out_dev, VIDIOC_G_AUDOUT);
+ v4l2_disable_ioctl(&dev->meta_out_dev, VIDIOC_ENUMAUDOUT);
+ }
+ if (!in_type_counter[TV] && !in_type_counter[SVID]) {
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_STD);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_STD);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_ENUMSTD);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_QUERYSTD);
+ }
+ if (!out_type_counter[SVID]) {
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_STD);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_STD);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUMSTD);
+ }
+ if (!has_tuner && !has_modulator) {
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_FREQUENCY);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_FREQUENCY);
+ v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_FREQUENCY);
+ v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_G_FREQUENCY);
+ v4l2_disable_ioctl(&dev->meta_cap_dev, VIDIOC_S_FREQUENCY);
+ v4l2_disable_ioctl(&dev->meta_cap_dev, VIDIOC_G_FREQUENCY);
+ }
+ if (!has_tuner) {
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_TUNER);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_TUNER);
+ v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_TUNER);
+ v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_G_TUNER);
+ v4l2_disable_ioctl(&dev->meta_cap_dev, VIDIOC_S_TUNER);
+ v4l2_disable_ioctl(&dev->meta_cap_dev, VIDIOC_G_TUNER);
+ }
+ if (in_type_counter[HDMI] == 0) {
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_EDID);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_EDID);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_DV_TIMINGS_CAP);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_DV_TIMINGS);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_DV_TIMINGS);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_ENUM_DV_TIMINGS);
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_QUERY_DV_TIMINGS);
+ }
+ if (out_type_counter[HDMI] == 0) {
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_EDID);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_DV_TIMINGS_CAP);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_DV_TIMINGS);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_DV_TIMINGS);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUM_DV_TIMINGS);
+ }
+ if (!dev->has_fb) {
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_FBUF);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_FBUF);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_OVERLAY);
+ }
+ v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+ v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+ v4l2_disable_ioctl(&dev->sdr_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+ v4l2_disable_ioctl(&dev->meta_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_FREQUENCY);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_FREQUENCY);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUM_FRAMESIZES);
+ v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUM_FRAMEINTERVALS);
+ v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_S_FREQUENCY);
+ v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_G_FREQUENCY);
+ v4l2_disable_ioctl(&dev->meta_out_dev, VIDIOC_S_FREQUENCY);
+ v4l2_disable_ioctl(&dev->meta_out_dev, VIDIOC_G_FREQUENCY);
+ v4l2_disable_ioctl(&dev->touch_cap_dev, VIDIOC_S_PARM);
+ v4l2_disable_ioctl(&dev->touch_cap_dev, VIDIOC_ENUM_FRAMESIZES);
+ v4l2_disable_ioctl(&dev->touch_cap_dev, VIDIOC_ENUM_FRAMEINTERVALS);
+}
+
+static int vivid_init_dv_timings(struct vivid_dev *dev)
+{
+ int i;
+
+ while (v4l2_dv_timings_presets[dev->query_dv_timings_size].bt.width)
+ dev->query_dv_timings_size++;
+
+ /*
+ * Create a char pointer array that points to the names of all the
+ * preset timings
+ */
+ dev->query_dv_timings_qmenu = kmalloc_array(dev->query_dv_timings_size,
+ sizeof(char *), GFP_KERNEL);
+ /*
+ * Create a string array containing the names of all the preset
+ * timings. Each name is max 31 chars long (+ terminating 0).
+ */
+ dev->query_dv_timings_qmenu_strings =
+ kmalloc_array(dev->query_dv_timings_size, 32, GFP_KERNEL);
+
+ if (!dev->query_dv_timings_qmenu ||
+ !dev->query_dv_timings_qmenu_strings)
+ return -ENOMEM;
+
+ for (i = 0; i < dev->query_dv_timings_size; i++) {
+ const struct v4l2_bt_timings *bt = &v4l2_dv_timings_presets[i].bt;
+ char *p = dev->query_dv_timings_qmenu_strings + i * 32;
+ u32 htot, vtot;
+
+ dev->query_dv_timings_qmenu[i] = p;
+
+ htot = V4L2_DV_BT_FRAME_WIDTH(bt);
+ vtot = V4L2_DV_BT_FRAME_HEIGHT(bt);
+ snprintf(p, 32, "%ux%u%s%u",
+ bt->width, bt->height, bt->interlaced ? "i" : "p",
+ (u32)bt->pixelclock / (htot * vtot));
+ }
+
+ return 0;
+}
+
+static int vivid_create_queues(struct vivid_dev *dev)
+{
+ int ret;
+
+ /* start creating the vb2 queues */
+ if (dev->has_vid_cap) {
+ /* initialize vid_cap queue */
+ ret = vivid_create_queue(dev, &dev->vb_vid_cap_q,
+ V4L2_BUF_TYPE_VIDEO_CAPTURE, 2,
+ &vivid_vid_cap_qops);
+ if (ret)
+ return ret;
+ }
+
+ if (dev->has_vid_out) {
+ /* initialize vid_out queue */
+ ret = vivid_create_queue(dev, &dev->vb_vid_out_q,
+ V4L2_BUF_TYPE_VIDEO_OUTPUT, 2,
+ &vivid_vid_out_qops);
+ if (ret)
+ return ret;
+ }
+
+ if (dev->has_vbi_cap) {
+ /* initialize vbi_cap queue */
+ ret = vivid_create_queue(dev, &dev->vb_vbi_cap_q,
+ V4L2_BUF_TYPE_VBI_CAPTURE, 2,
+ &vivid_vbi_cap_qops);
+ if (ret)
+ return ret;
+ }
+
+ if (dev->has_vbi_out) {
+ /* initialize vbi_out queue */
+ ret = vivid_create_queue(dev, &dev->vb_vbi_out_q,
+ V4L2_BUF_TYPE_VBI_OUTPUT, 2,
+ &vivid_vbi_out_qops);
+ if (ret)
+ return ret;
+ }
+
+ if (dev->has_sdr_cap) {
+ /* initialize sdr_cap queue */
+ ret = vivid_create_queue(dev, &dev->vb_sdr_cap_q,
+ V4L2_BUF_TYPE_SDR_CAPTURE, 8,
+ &vivid_sdr_cap_qops);
+ if (ret)
+ return ret;
+ }
+
+ if (dev->has_meta_cap) {
+ /* initialize meta_cap queue */
+ ret = vivid_create_queue(dev, &dev->vb_meta_cap_q,
+ V4L2_BUF_TYPE_META_CAPTURE, 2,
+ &vivid_meta_cap_qops);
+ if (ret)
+ return ret;
+ }
+
+ if (dev->has_meta_out) {
+ /* initialize meta_out queue */
+ ret = vivid_create_queue(dev, &dev->vb_meta_out_q,
+ V4L2_BUF_TYPE_META_OUTPUT, 1,
+ &vivid_meta_out_qops);
+ if (ret)
+ return ret;
+ }
+
+ if (dev->has_touch_cap) {
+ /* initialize touch_cap queue */
+ ret = vivid_create_queue(dev, &dev->vb_touch_cap_q,
+ V4L2_BUF_TYPE_VIDEO_CAPTURE, 1,
+ &vivid_touch_cap_qops);
+ if (ret)
+ return ret;
+ }
+
+ if (dev->has_fb) {
+ /* Create framebuffer for testing capture/output overlay */
+ ret = vivid_fb_init(dev);
+ if (ret)
+ return ret;
+ v4l2_info(&dev->v4l2_dev, "Framebuffer device registered as fb%d\n",
+ dev->fb_info.node);
+ }
+ return 0;
+}
+
+static int vivid_create_devnodes(struct platform_device *pdev,
+ struct vivid_dev *dev, int inst,
+ unsigned int cec_tx_bus_cnt,
+ v4l2_std_id tvnorms_cap,
+ v4l2_std_id tvnorms_out,
+ unsigned in_type_counter[4],
+ unsigned out_type_counter[4])
+{
+ struct video_device *vfd;
+ int ret;
+
+ if (dev->has_vid_cap) {
+ vfd = &dev->vid_cap_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-vid-cap", inst);
+ vfd->fops = &vivid_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->vid_cap_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->queue = &dev->vb_vid_cap_q;
+ vfd->tvnorms = tvnorms_cap;
+
+ /*
+ * Provide a mutex to v4l2 core. It will be used to protect
+ * all fops and v4l2 ioctls.
+ */
+ vfd->lock = &dev->mutex;
+ video_set_drvdata(vfd, dev);
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->vid_cap_pad.flags = MEDIA_PAD_FL_SINK;
+ ret = media_entity_pads_init(&vfd->entity, 1, &dev->vid_cap_pad);
+ if (ret)
+ return ret;
+#endif
+
+#ifdef CONFIG_VIDEO_VIVID_CEC
+ if (in_type_counter[HDMI]) {
+ ret = cec_register_adapter(dev->cec_rx_adap, &pdev->dev);
+ if (ret < 0) {
+ cec_delete_adapter(dev->cec_rx_adap);
+ dev->cec_rx_adap = NULL;
+ return ret;
+ }
+ cec_s_phys_addr(dev->cec_rx_adap, 0, false);
+ v4l2_info(&dev->v4l2_dev, "CEC adapter %s registered for HDMI input 0\n",
+ dev_name(&dev->cec_rx_adap->devnode.dev));
+ }
+#endif
+
+ ret = video_register_device(vfd, VFL_TYPE_VIDEO, vid_cap_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev, "V4L2 capture device registered as %s\n",
+ video_device_node_name(vfd));
+ }
+
+ if (dev->has_vid_out) {
+#ifdef CONFIG_VIDEO_VIVID_CEC
+ int i;
+#endif
+ vfd = &dev->vid_out_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-vid-out", inst);
+ vfd->vfl_dir = VFL_DIR_TX;
+ vfd->fops = &vivid_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->vid_out_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->queue = &dev->vb_vid_out_q;
+ vfd->tvnorms = tvnorms_out;
+
+ /*
+ * Provide a mutex to v4l2 core. It will be used to protect
+ * all fops and v4l2 ioctls.
+ */
+ vfd->lock = &dev->mutex;
+ video_set_drvdata(vfd, dev);
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->vid_out_pad.flags = MEDIA_PAD_FL_SOURCE;
+ ret = media_entity_pads_init(&vfd->entity, 1, &dev->vid_out_pad);
+ if (ret)
+ return ret;
+#endif
+
+#ifdef CONFIG_VIDEO_VIVID_CEC
+ for (i = 0; i < cec_tx_bus_cnt; i++) {
+ ret = cec_register_adapter(dev->cec_tx_adap[i], &pdev->dev);
+ if (ret < 0) {
+ for (; i < cec_tx_bus_cnt; i++) {
+ cec_delete_adapter(dev->cec_tx_adap[i]);
+ dev->cec_tx_adap[i] = NULL;
+ }
+ return ret;
+ }
+ v4l2_info(&dev->v4l2_dev, "CEC adapter %s registered for HDMI output %d\n",
+ dev_name(&dev->cec_tx_adap[i]->devnode.dev), i);
+ if (i < out_type_counter[HDMI])
+ cec_s_phys_addr(dev->cec_tx_adap[i], (i + 1) << 12, false);
+ else
+ cec_s_phys_addr(dev->cec_tx_adap[i], 0x1000, false);
+ }
+#endif
+
+ ret = video_register_device(vfd, VFL_TYPE_VIDEO, vid_out_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev, "V4L2 output device registered as %s\n",
+ video_device_node_name(vfd));
+ }
+
+ if (dev->has_vbi_cap) {
+ vfd = &dev->vbi_cap_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-vbi-cap", inst);
+ vfd->fops = &vivid_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->vbi_cap_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->queue = &dev->vb_vbi_cap_q;
+ vfd->lock = &dev->mutex;
+ vfd->tvnorms = tvnorms_cap;
+ video_set_drvdata(vfd, dev);
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->vbi_cap_pad.flags = MEDIA_PAD_FL_SINK;
+ ret = media_entity_pads_init(&vfd->entity, 1, &dev->vbi_cap_pad);
+ if (ret)
+ return ret;
+#endif
+
+ ret = video_register_device(vfd, VFL_TYPE_VBI, vbi_cap_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev, "V4L2 capture device registered as %s, supports %s VBI\n",
+ video_device_node_name(vfd),
+ (dev->has_raw_vbi_cap && dev->has_sliced_vbi_cap) ?
+ "raw and sliced" :
+ (dev->has_raw_vbi_cap ? "raw" : "sliced"));
+ }
+
+ if (dev->has_vbi_out) {
+ vfd = &dev->vbi_out_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-vbi-out", inst);
+ vfd->vfl_dir = VFL_DIR_TX;
+ vfd->fops = &vivid_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->vbi_out_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->queue = &dev->vb_vbi_out_q;
+ vfd->lock = &dev->mutex;
+ vfd->tvnorms = tvnorms_out;
+ video_set_drvdata(vfd, dev);
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->vbi_out_pad.flags = MEDIA_PAD_FL_SOURCE;
+ ret = media_entity_pads_init(&vfd->entity, 1, &dev->vbi_out_pad);
+ if (ret)
+ return ret;
+#endif
+
+ ret = video_register_device(vfd, VFL_TYPE_VBI, vbi_out_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev, "V4L2 output device registered as %s, supports %s VBI\n",
+ video_device_node_name(vfd),
+ (dev->has_raw_vbi_out && dev->has_sliced_vbi_out) ?
+ "raw and sliced" :
+ (dev->has_raw_vbi_out ? "raw" : "sliced"));
+ }
+
+ if (dev->has_sdr_cap) {
+ vfd = &dev->sdr_cap_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-sdr-cap", inst);
+ vfd->fops = &vivid_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->sdr_cap_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->queue = &dev->vb_sdr_cap_q;
+ vfd->lock = &dev->mutex;
+ video_set_drvdata(vfd, dev);
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->sdr_cap_pad.flags = MEDIA_PAD_FL_SINK;
+ ret = media_entity_pads_init(&vfd->entity, 1, &dev->sdr_cap_pad);
+ if (ret)
+ return ret;
+#endif
+
+ ret = video_register_device(vfd, VFL_TYPE_SDR, sdr_cap_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev, "V4L2 capture device registered as %s\n",
+ video_device_node_name(vfd));
+ }
+
+ if (dev->has_radio_rx) {
+ vfd = &dev->radio_rx_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-rad-rx", inst);
+ vfd->fops = &vivid_radio_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->radio_rx_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->lock = &dev->mutex;
+ video_set_drvdata(vfd, dev);
+
+ ret = video_register_device(vfd, VFL_TYPE_RADIO, radio_rx_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev, "V4L2 receiver device registered as %s\n",
+ video_device_node_name(vfd));
+ }
+
+ if (dev->has_radio_tx) {
+ vfd = &dev->radio_tx_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-rad-tx", inst);
+ vfd->vfl_dir = VFL_DIR_TX;
+ vfd->fops = &vivid_radio_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->radio_tx_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->lock = &dev->mutex;
+ video_set_drvdata(vfd, dev);
+
+ ret = video_register_device(vfd, VFL_TYPE_RADIO, radio_tx_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev, "V4L2 transmitter device registered as %s\n",
+ video_device_node_name(vfd));
+ }
+
+ if (dev->has_meta_cap) {
+ vfd = &dev->meta_cap_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-meta-cap", inst);
+ vfd->fops = &vivid_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->meta_cap_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->queue = &dev->vb_meta_cap_q;
+ vfd->lock = &dev->mutex;
+ vfd->tvnorms = tvnorms_cap;
+ video_set_drvdata(vfd, dev);
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->meta_cap_pad.flags = MEDIA_PAD_FL_SINK;
+ ret = media_entity_pads_init(&vfd->entity, 1,
+ &dev->meta_cap_pad);
+ if (ret)
+ return ret;
+#endif
+ ret = video_register_device(vfd, VFL_TYPE_VIDEO,
+ meta_cap_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev,
+ "V4L2 metadata capture device registered as %s\n",
+ video_device_node_name(vfd));
+ }
+
+ if (dev->has_meta_out) {
+ vfd = &dev->meta_out_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-meta-out", inst);
+ vfd->vfl_dir = VFL_DIR_TX;
+ vfd->fops = &vivid_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->meta_out_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->queue = &dev->vb_meta_out_q;
+ vfd->lock = &dev->mutex;
+ vfd->tvnorms = tvnorms_out;
+ video_set_drvdata(vfd, dev);
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->meta_out_pad.flags = MEDIA_PAD_FL_SOURCE;
+ ret = media_entity_pads_init(&vfd->entity, 1,
+ &dev->meta_out_pad);
+ if (ret)
+ return ret;
+#endif
+ ret = video_register_device(vfd, VFL_TYPE_VIDEO,
+ meta_out_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev,
+ "V4L2 metadata output device registered as %s\n",
+ video_device_node_name(vfd));
+ }
+
+ if (dev->has_touch_cap) {
+ vfd = &dev->touch_cap_dev;
+ snprintf(vfd->name, sizeof(vfd->name),
+ "vivid-%03d-touch-cap", inst);
+ vfd->fops = &vivid_fops;
+ vfd->ioctl_ops = &vivid_ioctl_ops;
+ vfd->device_caps = dev->touch_cap_caps;
+ vfd->release = video_device_release_empty;
+ vfd->v4l2_dev = &dev->v4l2_dev;
+ vfd->queue = &dev->vb_touch_cap_q;
+ vfd->tvnorms = tvnorms_cap;
+ vfd->lock = &dev->mutex;
+ video_set_drvdata(vfd, dev);
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->touch_cap_pad.flags = MEDIA_PAD_FL_SINK;
+ ret = media_entity_pads_init(&vfd->entity, 1,
+ &dev->touch_cap_pad);
+ if (ret)
+ return ret;
+#endif
+ ret = video_register_device(vfd, VFL_TYPE_TOUCH,
+ touch_cap_nr[inst]);
+ if (ret < 0)
+ return ret;
+ v4l2_info(&dev->v4l2_dev,
+ "V4L2 touch capture device registered as %s\n",
+ video_device_node_name(vfd));
+ }
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ /* Register the media device */
+ ret = media_device_register(&dev->mdev);
+ if (ret) {
+ dev_err(dev->mdev.dev,
+ "media device register failed (err=%d)\n", ret);
+ return ret;
+ }
+#endif
+ return 0;
+}
+
+static int vivid_create_instance(struct platform_device *pdev, int inst)
+{
+ static const struct v4l2_dv_timings def_dv_timings =
+ V4L2_DV_BT_CEA_1280X720P60;
+ unsigned in_type_counter[4] = { 0, 0, 0, 0 };
+ unsigned out_type_counter[4] = { 0, 0, 0, 0 };
+ int ccs_cap = ccs_cap_mode[inst];
+ int ccs_out = ccs_out_mode[inst];
+ bool has_tuner;
+ bool has_modulator;
+ struct vivid_dev *dev;
+ unsigned node_type = node_types[inst];
+ v4l2_std_id tvnorms_cap = 0, tvnorms_out = 0;
+ unsigned int cec_tx_bus_cnt = 0;
+ int ret;
+ int i;
+
+ /* allocate main vivid state structure */
+ dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+ if (!dev)
+ return -ENOMEM;
+
+ dev->inst = inst;
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ dev->v4l2_dev.mdev = &dev->mdev;
+
+ /* Initialize media device */
+ strscpy(dev->mdev.model, VIVID_MODULE_NAME, sizeof(dev->mdev.model));
+ snprintf(dev->mdev.bus_info, sizeof(dev->mdev.bus_info),
+ "platform:%s-%03d", VIVID_MODULE_NAME, inst);
+ dev->mdev.dev = &pdev->dev;
+ media_device_init(&dev->mdev);
+ dev->mdev.ops = &vivid_media_ops;
+#endif
+
+ /* register v4l2_device */
+ snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name),
+ "%s-%03d", VIVID_MODULE_NAME, inst);
+ ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+ if (ret) {
+ kfree(dev);
+ return ret;
+ }
+ dev->v4l2_dev.release = vivid_dev_release;
+
+ ret = vivid_detect_feature_set(dev, inst, node_type,
+ &has_tuner, &has_modulator,
+ &ccs_cap, &ccs_out,
+ in_type_counter, out_type_counter);
+ if (ret)
+ goto free_dev;
+
+ vivid_set_capabilities(dev);
+
+ ret = -ENOMEM;
+ /* initialize the test pattern generator */
+ tpg_init(&dev->tpg, 640, 360);
+ if (tpg_alloc(&dev->tpg, array_size(MAX_WIDTH, MAX_ZOOM)))
+ goto free_dev;
+ dev->scaled_line = vzalloc(array_size(MAX_WIDTH, MAX_ZOOM));
+ if (!dev->scaled_line)
+ goto free_dev;
+ dev->blended_line = vzalloc(array_size(MAX_WIDTH, MAX_ZOOM));
+ if (!dev->blended_line)
+ goto free_dev;
+
+ /* load the edid */
+ dev->edid = vmalloc(array_size(256, 128));
+ if (!dev->edid)
+ goto free_dev;
+
+ ret = vivid_init_dv_timings(dev);
+ if (ret < 0)
+ goto free_dev;
+
+ vivid_disable_unused_ioctls(dev, has_tuner, has_modulator,
+ in_type_counter, out_type_counter);
+
+ /* configure internal data */
+ dev->fmt_cap = &vivid_formats[0];
+ dev->fmt_out = &vivid_formats[0];
+ if (!dev->multiplanar)
+ vivid_formats[0].data_offset[0] = 0;
+ dev->webcam_size_idx = 1;
+ dev->webcam_ival_idx = 3;
+ tpg_s_fourcc(&dev->tpg, dev->fmt_cap->fourcc);
+ dev->std_out = V4L2_STD_PAL;
+ if (dev->input_type[0] == TV || dev->input_type[0] == SVID)
+ tvnorms_cap = V4L2_STD_ALL;
+ if (dev->output_type[0] == SVID)
+ tvnorms_out = V4L2_STD_ALL;
+ for (i = 0; i < MAX_INPUTS; i++) {
+ dev->dv_timings_cap[i] = def_dv_timings;
+ dev->std_cap[i] = V4L2_STD_PAL;
+ }
+ dev->dv_timings_out = def_dv_timings;
+ dev->tv_freq = 2804 /* 175.25 * 16 */;
+ dev->tv_audmode = V4L2_TUNER_MODE_STEREO;
+ dev->tv_field_cap = V4L2_FIELD_INTERLACED;
+ dev->tv_field_out = V4L2_FIELD_INTERLACED;
+ dev->radio_rx_freq = 95000 * 16;
+ dev->radio_rx_audmode = V4L2_TUNER_MODE_STEREO;
+ if (dev->has_radio_tx) {
+ dev->radio_tx_freq = 95500 * 16;
+ dev->radio_rds_loop = false;
+ }
+ dev->radio_tx_subchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_RDS;
+ dev->sdr_adc_freq = 300000;
+ dev->sdr_fm_freq = 50000000;
+ dev->sdr_pixelformat = V4L2_SDR_FMT_CU8;
+ dev->sdr_buffersize = SDR_CAP_SAMPLES_PER_BUF * 2;
+
+ dev->edid_max_blocks = dev->edid_blocks = 2;
+ memcpy(dev->edid, vivid_hdmi_edid, sizeof(vivid_hdmi_edid));
+ dev->radio_rds_init_time = ktime_get();
+
+ /* create all controls */
+ ret = vivid_create_controls(dev, ccs_cap == -1, ccs_out == -1, no_error_inj,
+ in_type_counter[TV] || in_type_counter[SVID] ||
+ out_type_counter[SVID],
+ in_type_counter[HDMI] || out_type_counter[HDMI]);
+ if (ret)
+ goto unreg_dev;
+
+ /* enable/disable interface specific controls */
+ if (dev->num_outputs && dev->output_type[0] != HDMI)
+ v4l2_ctrl_activate(dev->ctrl_display_present, false);
+ if (dev->num_inputs && dev->input_type[0] != HDMI) {
+ v4l2_ctrl_activate(dev->ctrl_dv_timings_signal_mode, false);
+ v4l2_ctrl_activate(dev->ctrl_dv_timings, false);
+ } else if (dev->num_inputs && dev->input_type[0] == HDMI) {
+ v4l2_ctrl_activate(dev->ctrl_std_signal_mode, false);
+ v4l2_ctrl_activate(dev->ctrl_standard, false);
+ }
+
+ /*
+ * update the capture and output formats to do a proper initial
+ * configuration.
+ */
+ vivid_update_format_cap(dev, false);
+ vivid_update_format_out(dev);
+
+ /* initialize overlay */
+ dev->fb_cap.fmt.width = dev->src_rect.width;
+ dev->fb_cap.fmt.height = dev->src_rect.height;
+ dev->fb_cap.fmt.pixelformat = dev->fmt_cap->fourcc;
+ dev->fb_cap.fmt.bytesperline = dev->src_rect.width * tpg_g_twopixelsize(&dev->tpg, 0) / 2;
+ dev->fb_cap.fmt.sizeimage = dev->src_rect.height * dev->fb_cap.fmt.bytesperline;
+
+ /* update touch configuration */
+ dev->timeperframe_tch_cap.numerator = 1;
+ dev->timeperframe_tch_cap.denominator = 10;
+ vivid_set_touch(dev, 0);
+
+ /* initialize locks */
+ spin_lock_init(&dev->slock);
+ mutex_init(&dev->mutex);
+
+ /* init dma queues */
+ INIT_LIST_HEAD(&dev->vid_cap_active);
+ INIT_LIST_HEAD(&dev->vid_out_active);
+ INIT_LIST_HEAD(&dev->vbi_cap_active);
+ INIT_LIST_HEAD(&dev->vbi_out_active);
+ INIT_LIST_HEAD(&dev->sdr_cap_active);
+ INIT_LIST_HEAD(&dev->meta_cap_active);
+ INIT_LIST_HEAD(&dev->meta_out_active);
+ INIT_LIST_HEAD(&dev->touch_cap_active);
+
+ INIT_LIST_HEAD(&dev->cec_work_list);
+ spin_lock_init(&dev->cec_slock);
+ /*
+ * Same as create_singlethread_workqueue, but now I can use the
+ * string formatting of alloc_ordered_workqueue.
+ */
+ dev->cec_workqueue = alloc_ordered_workqueue("vivid-%03d-cec",
+ WQ_MEM_RECLAIM, inst);
+ if (!dev->cec_workqueue) {
+ ret = -ENOMEM;
+ goto unreg_dev;
+ }
+
+ if (allocators[inst] == 1)
+ dma_coerce_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
+
+ ret = vivid_create_queues(dev);
+ if (ret)
+ goto unreg_dev;
+
+#ifdef CONFIG_VIDEO_VIVID_CEC
+ if (dev->has_vid_cap && in_type_counter[HDMI]) {
+ struct cec_adapter *adap;
+
+ adap = vivid_cec_alloc_adap(dev, 0, false);
+ ret = PTR_ERR_OR_ZERO(adap);
+ if (ret < 0)
+ goto unreg_dev;
+ dev->cec_rx_adap = adap;
+ }
+
+ if (dev->has_vid_out) {
+ for (i = 0; i < dev->num_outputs; i++) {
+ struct cec_adapter *adap;
+
+ if (dev->output_type[i] != HDMI)
+ continue;
+
+ dev->cec_output2bus_map[i] = cec_tx_bus_cnt;
+ adap = vivid_cec_alloc_adap(dev, cec_tx_bus_cnt, true);
+ ret = PTR_ERR_OR_ZERO(adap);
+ if (ret < 0) {
+ for (i = 0; i < dev->num_outputs; i++)
+ cec_delete_adapter(dev->cec_tx_adap[i]);
+ goto unreg_dev;
+ }
+
+ dev->cec_tx_adap[cec_tx_bus_cnt] = adap;
+ cec_tx_bus_cnt++;
+ }
+ }
+#endif
+
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vid_cap);
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vid_out);
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vbi_cap);
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vbi_out);
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_radio_rx);
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_radio_tx);
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_sdr_cap);
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_meta_cap);
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_meta_out);
+ v4l2_ctrl_handler_setup(&dev->ctrl_hdl_touch_cap);
+
+ /* finally start creating the device nodes */
+ ret = vivid_create_devnodes(pdev, dev, inst, cec_tx_bus_cnt,
+ tvnorms_cap, tvnorms_out,
+ in_type_counter, out_type_counter);
+ if (ret)
+ goto unreg_dev;
+
+ /* Now that everything is fine, let's add it to device list */
+ vivid_devs[inst] = dev;
+
+ return 0;
+
+unreg_dev:
+ vb2_video_unregister_device(&dev->touch_cap_dev);
+ vb2_video_unregister_device(&dev->meta_out_dev);
+ vb2_video_unregister_device(&dev->meta_cap_dev);
+ video_unregister_device(&dev->radio_tx_dev);
+ video_unregister_device(&dev->radio_rx_dev);
+ vb2_video_unregister_device(&dev->sdr_cap_dev);
+ vb2_video_unregister_device(&dev->vbi_out_dev);
+ vb2_video_unregister_device(&dev->vbi_cap_dev);
+ vb2_video_unregister_device(&dev->vid_out_dev);
+ vb2_video_unregister_device(&dev->vid_cap_dev);
+ cec_unregister_adapter(dev->cec_rx_adap);
+ for (i = 0; i < MAX_OUTPUTS; i++)
+ cec_unregister_adapter(dev->cec_tx_adap[i]);
+ if (dev->cec_workqueue) {
+ vivid_cec_bus_free_work(dev);
+ destroy_workqueue(dev->cec_workqueue);
+ }
+free_dev:
+ v4l2_device_put(&dev->v4l2_dev);
+ return ret;
+}
+
+/* This routine allocates from 1 to n_devs virtual drivers.
+
+ The real maximum number of virtual drivers will depend on how many drivers
+ will succeed. This is limited to the maximum number of devices that
+ videodev supports, which is equal to VIDEO_NUM_DEVICES.
+ */
+static int vivid_probe(struct platform_device *pdev)
+{
+ const struct font_desc *font = find_font("VGA8x16");
+ int ret = 0, i;
+
+ if (font == NULL) {
+ pr_err("vivid: could not find font\n");
+ return -ENODEV;
+ }
+
+ tpg_set_font(font->data);
+
+ n_devs = clamp_t(unsigned, n_devs, 1, VIVID_MAX_DEVS);
+
+ for (i = 0; i < n_devs; i++) {
+ ret = vivid_create_instance(pdev, i);
+ if (ret) {
+ /* If some instantiations succeeded, keep driver */
+ if (i)
+ ret = 0;
+ break;
+ }
+ }
+
+ if (ret < 0) {
+ pr_err("vivid: error %d while loading driver\n", ret);
+ return ret;
+ }
+
+ /* n_devs will reflect the actual number of allocated devices */
+ n_devs = i;
+
+ return ret;
+}
+
+static int vivid_remove(struct platform_device *pdev)
+{
+ struct vivid_dev *dev;
+ unsigned int i, j;
+
+ for (i = 0; i < n_devs; i++) {
+ dev = vivid_devs[i];
+ if (!dev)
+ continue;
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+ media_device_unregister(&dev->mdev);
+#endif
+
+ if (dev->has_vid_cap) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->vid_cap_dev));
+ vb2_video_unregister_device(&dev->vid_cap_dev);
+ }
+ if (dev->has_vid_out) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->vid_out_dev));
+ vb2_video_unregister_device(&dev->vid_out_dev);
+ }
+ if (dev->has_vbi_cap) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->vbi_cap_dev));
+ vb2_video_unregister_device(&dev->vbi_cap_dev);
+ }
+ if (dev->has_vbi_out) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->vbi_out_dev));
+ vb2_video_unregister_device(&dev->vbi_out_dev);
+ }
+ if (dev->has_sdr_cap) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->sdr_cap_dev));
+ vb2_video_unregister_device(&dev->sdr_cap_dev);
+ }
+ if (dev->has_radio_rx) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->radio_rx_dev));
+ video_unregister_device(&dev->radio_rx_dev);
+ }
+ if (dev->has_radio_tx) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->radio_tx_dev));
+ video_unregister_device(&dev->radio_tx_dev);
+ }
+ if (dev->has_fb) {
+ v4l2_info(&dev->v4l2_dev, "unregistering fb%d\n",
+ dev->fb_info.node);
+ unregister_framebuffer(&dev->fb_info);
+ vivid_fb_release_buffers(dev);
+ }
+ if (dev->has_meta_cap) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->meta_cap_dev));
+ vb2_video_unregister_device(&dev->meta_cap_dev);
+ }
+ if (dev->has_meta_out) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->meta_out_dev));
+ vb2_video_unregister_device(&dev->meta_out_dev);
+ }
+ if (dev->has_touch_cap) {
+ v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+ video_device_node_name(&dev->touch_cap_dev));
+ vb2_video_unregister_device(&dev->touch_cap_dev);
+ }
+ cec_unregister_adapter(dev->cec_rx_adap);
+ for (j = 0; j < MAX_OUTPUTS; j++)
+ cec_unregister_adapter(dev->cec_tx_adap[j]);
+ if (dev->cec_workqueue) {
+ vivid_cec_bus_free_work(dev);
+ destroy_workqueue(dev->cec_workqueue);
+ }
+ v4l2_device_put(&dev->v4l2_dev);
+ vivid_devs[i] = NULL;
+ }
+ return 0;
+}
+
+static void vivid_pdev_release(struct device *dev)
+{
+}
+
+static struct platform_device vivid_pdev = {
+ .name = "vivid",
+ .dev.release = vivid_pdev_release,
+};
+
+static struct platform_driver vivid_pdrv = {
+ .probe = vivid_probe,
+ .remove = vivid_remove,
+ .driver = {
+ .name = "vivid",
+ },
+};
+
+static int __init vivid_init(void)
+{
+ int ret;
+
+ ret = platform_device_register(&vivid_pdev);
+ if (ret)
+ return ret;
+
+ ret = platform_driver_register(&vivid_pdrv);
+ if (ret)
+ platform_device_unregister(&vivid_pdev);
+
+ return ret;
+}
+
+static void __exit vivid_exit(void)
+{
+ platform_driver_unregister(&vivid_pdrv);
+ platform_device_unregister(&vivid_pdev);
+}
+
+module_init(vivid_init);
+module_exit(vivid_exit);
diff --git a/drivers/media/test-drivers/vivid/vivid-core.h b/drivers/media/test-drivers/vivid/vivid-core.h
new file mode 100644
index 0000000..99e69b8
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-core.h
@@ -0,0 +1,612 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-core.h - core datastructures
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_CORE_H_
+#define _VIVID_CORE_H_
+
+#include <linux/fb.h>
+#include <linux/workqueue.h>
+#include <media/cec.h>
+#include <media/videobuf2-v4l2.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ctrls.h>
+#include <media/tpg/v4l2-tpg.h>
+#include "vivid-rds-gen.h"
+#include "vivid-vbi-gen.h"
+
+#define dprintk(dev, level, fmt, arg...) \
+ v4l2_dbg(level, vivid_debug, &dev->v4l2_dev, fmt, ## arg)
+
+/* The maximum number of clip rectangles */
+#define MAX_CLIPS 16
+/* The maximum number of inputs */
+#define MAX_INPUTS 16
+/* The maximum number of outputs */
+#define MAX_OUTPUTS 16
+/* The maximum up or down scaling factor is 4 */
+#define MAX_ZOOM 4
+/* The maximum image width/height are set to 4K DMT */
+#define MAX_WIDTH 4096
+#define MAX_HEIGHT 2160
+/* The minimum image width/height */
+#define MIN_WIDTH 16
+#define MIN_HEIGHT 16
+/* The data_offset of plane 0 for the multiplanar formats */
+#define PLANE0_DATA_OFFSET 128
+
+/* The supported TV frequency range in MHz */
+#define MIN_TV_FREQ (44U * 16U)
+#define MAX_TV_FREQ (958U * 16U)
+
+/* The number of samples returned in every SDR buffer */
+#define SDR_CAP_SAMPLES_PER_BUF 0x4000
+
+/* used by the threads to know when to resync internal counters */
+#define JIFFIES_PER_DAY (3600U * 24U * HZ)
+#define JIFFIES_RESYNC (JIFFIES_PER_DAY * (0xf0000000U / JIFFIES_PER_DAY))
+
+extern const struct v4l2_rect vivid_min_rect;
+extern const struct v4l2_rect vivid_max_rect;
+extern unsigned vivid_debug;
+
+struct vivid_fmt {
+ u32 fourcc; /* v4l2 format id */
+ enum tgp_color_enc color_enc;
+ bool can_do_overlay;
+ u8 vdownsampling[TPG_MAX_PLANES];
+ u32 alpha_mask;
+ u8 planes;
+ u8 buffers;
+ u32 data_offset[TPG_MAX_PLANES];
+ u32 bit_depth[TPG_MAX_PLANES];
+};
+
+extern struct vivid_fmt vivid_formats[];
+
+/* buffer for one video frame */
+struct vivid_buffer {
+ /* common v4l buffer stuff -- must be first */
+ struct vb2_v4l2_buffer vb;
+ struct list_head list;
+};
+
+enum vivid_input {
+ WEBCAM,
+ TV,
+ SVID,
+ HDMI,
+};
+
+enum vivid_signal_mode {
+ CURRENT_DV_TIMINGS,
+ CURRENT_STD = CURRENT_DV_TIMINGS,
+ NO_SIGNAL,
+ NO_LOCK,
+ OUT_OF_RANGE,
+ SELECTED_DV_TIMINGS,
+ SELECTED_STD = SELECTED_DV_TIMINGS,
+ CYCLE_DV_TIMINGS,
+ CYCLE_STD = CYCLE_DV_TIMINGS,
+ CUSTOM_DV_TIMINGS,
+};
+
+enum vivid_colorspace {
+ VIVID_CS_170M,
+ VIVID_CS_709,
+ VIVID_CS_SRGB,
+ VIVID_CS_OPRGB,
+ VIVID_CS_2020,
+ VIVID_CS_DCI_P3,
+ VIVID_CS_240M,
+ VIVID_CS_SYS_M,
+ VIVID_CS_SYS_BG,
+};
+
+#define VIVID_INVALID_SIGNAL(mode) \
+ ((mode) == NO_SIGNAL || (mode) == NO_LOCK || (mode) == OUT_OF_RANGE)
+
+struct vivid_cec_work {
+ struct list_head list;
+ struct delayed_work work;
+ struct cec_adapter *adap;
+ struct vivid_dev *dev;
+ unsigned int usecs;
+ unsigned int timeout_ms;
+ u8 tx_status;
+ struct cec_msg msg;
+};
+
+struct vivid_dev {
+ unsigned inst;
+ struct v4l2_device v4l2_dev;
+#ifdef CONFIG_MEDIA_CONTROLLER
+ struct media_device mdev;
+ struct media_pad vid_cap_pad;
+ struct media_pad vid_out_pad;
+ struct media_pad vbi_cap_pad;
+ struct media_pad vbi_out_pad;
+ struct media_pad sdr_cap_pad;
+ struct media_pad meta_cap_pad;
+ struct media_pad meta_out_pad;
+ struct media_pad touch_cap_pad;
+#endif
+ struct v4l2_ctrl_handler ctrl_hdl_user_gen;
+ struct v4l2_ctrl_handler ctrl_hdl_user_vid;
+ struct v4l2_ctrl_handler ctrl_hdl_user_aud;
+ struct v4l2_ctrl_handler ctrl_hdl_streaming;
+ struct v4l2_ctrl_handler ctrl_hdl_sdtv_cap;
+ struct v4l2_ctrl_handler ctrl_hdl_loop_cap;
+ struct v4l2_ctrl_handler ctrl_hdl_fb;
+ struct video_device vid_cap_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_vid_cap;
+ struct video_device vid_out_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_vid_out;
+ struct video_device vbi_cap_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_vbi_cap;
+ struct video_device vbi_out_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_vbi_out;
+ struct video_device radio_rx_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_radio_rx;
+ struct video_device radio_tx_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_radio_tx;
+ struct video_device sdr_cap_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_sdr_cap;
+ struct video_device meta_cap_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_meta_cap;
+ struct video_device meta_out_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_meta_out;
+ struct video_device touch_cap_dev;
+ struct v4l2_ctrl_handler ctrl_hdl_touch_cap;
+
+ spinlock_t slock;
+ struct mutex mutex;
+
+ /* capabilities */
+ u32 vid_cap_caps;
+ u32 vid_out_caps;
+ u32 vbi_cap_caps;
+ u32 vbi_out_caps;
+ u32 sdr_cap_caps;
+ u32 radio_rx_caps;
+ u32 radio_tx_caps;
+ u32 meta_cap_caps;
+ u32 meta_out_caps;
+ u32 touch_cap_caps;
+
+ /* supported features */
+ bool multiplanar;
+ unsigned num_inputs;
+ unsigned int num_hdmi_inputs;
+ u8 input_type[MAX_INPUTS];
+ u8 input_name_counter[MAX_INPUTS];
+ unsigned num_outputs;
+ unsigned int num_hdmi_outputs;
+ u8 output_type[MAX_OUTPUTS];
+ u8 output_name_counter[MAX_OUTPUTS];
+ bool has_audio_inputs;
+ bool has_audio_outputs;
+ bool has_vid_cap;
+ bool has_vid_out;
+ bool has_vbi_cap;
+ bool has_raw_vbi_cap;
+ bool has_sliced_vbi_cap;
+ bool has_vbi_out;
+ bool has_raw_vbi_out;
+ bool has_sliced_vbi_out;
+ bool has_radio_rx;
+ bool has_radio_tx;
+ bool has_sdr_cap;
+ bool has_fb;
+ bool has_meta_cap;
+ bool has_meta_out;
+ bool has_tv_tuner;
+ bool has_touch_cap;
+
+ bool can_loop_video;
+
+ /* controls */
+ struct v4l2_ctrl *brightness;
+ struct v4l2_ctrl *contrast;
+ struct v4l2_ctrl *saturation;
+ struct v4l2_ctrl *hue;
+ struct {
+ /* autogain/gain cluster */
+ struct v4l2_ctrl *autogain;
+ struct v4l2_ctrl *gain;
+ };
+ struct v4l2_ctrl *volume;
+ struct v4l2_ctrl *mute;
+ struct v4l2_ctrl *alpha;
+ struct v4l2_ctrl *button;
+ struct v4l2_ctrl *boolean;
+ struct v4l2_ctrl *int32;
+ struct v4l2_ctrl *int64;
+ struct v4l2_ctrl *menu;
+ struct v4l2_ctrl *string;
+ struct v4l2_ctrl *bitmask;
+ struct v4l2_ctrl *int_menu;
+ struct v4l2_ctrl *test_pattern;
+ struct v4l2_ctrl *colorspace;
+ struct v4l2_ctrl *rgb_range_cap;
+ struct v4l2_ctrl *real_rgb_range_cap;
+ struct {
+ /* std_signal_mode/standard cluster */
+ struct v4l2_ctrl *ctrl_std_signal_mode;
+ struct v4l2_ctrl *ctrl_standard;
+ };
+ struct {
+ /* dv_timings_signal_mode/timings cluster */
+ struct v4l2_ctrl *ctrl_dv_timings_signal_mode;
+ struct v4l2_ctrl *ctrl_dv_timings;
+ };
+ struct v4l2_ctrl *ctrl_display_present;
+ struct v4l2_ctrl *ctrl_has_crop_cap;
+ struct v4l2_ctrl *ctrl_has_compose_cap;
+ struct v4l2_ctrl *ctrl_has_scaler_cap;
+ struct v4l2_ctrl *ctrl_has_crop_out;
+ struct v4l2_ctrl *ctrl_has_compose_out;
+ struct v4l2_ctrl *ctrl_has_scaler_out;
+ struct v4l2_ctrl *ctrl_tx_mode;
+ struct v4l2_ctrl *ctrl_tx_rgb_range;
+ struct v4l2_ctrl *ctrl_tx_edid_present;
+ struct v4l2_ctrl *ctrl_tx_hotplug;
+ struct v4l2_ctrl *ctrl_tx_rxsense;
+
+ struct v4l2_ctrl *ctrl_rx_power_present;
+
+ struct v4l2_ctrl *radio_tx_rds_pi;
+ struct v4l2_ctrl *radio_tx_rds_pty;
+ struct v4l2_ctrl *radio_tx_rds_mono_stereo;
+ struct v4l2_ctrl *radio_tx_rds_art_head;
+ struct v4l2_ctrl *radio_tx_rds_compressed;
+ struct v4l2_ctrl *radio_tx_rds_dyn_pty;
+ struct v4l2_ctrl *radio_tx_rds_ta;
+ struct v4l2_ctrl *radio_tx_rds_tp;
+ struct v4l2_ctrl *radio_tx_rds_ms;
+ struct v4l2_ctrl *radio_tx_rds_psname;
+ struct v4l2_ctrl *radio_tx_rds_radiotext;
+
+ struct v4l2_ctrl *radio_rx_rds_pty;
+ struct v4l2_ctrl *radio_rx_rds_ta;
+ struct v4l2_ctrl *radio_rx_rds_tp;
+ struct v4l2_ctrl *radio_rx_rds_ms;
+ struct v4l2_ctrl *radio_rx_rds_psname;
+ struct v4l2_ctrl *radio_rx_rds_radiotext;
+
+ unsigned input_brightness[MAX_INPUTS];
+ unsigned osd_mode;
+ unsigned button_pressed;
+ bool sensor_hflip;
+ bool sensor_vflip;
+ bool hflip;
+ bool vflip;
+ bool vbi_cap_interlaced;
+ bool loop_video;
+ bool reduced_fps;
+
+ /* Framebuffer */
+ unsigned long video_pbase;
+ void *video_vbase;
+ u32 video_buffer_size;
+ int display_width;
+ int display_height;
+ int display_byte_stride;
+ int bits_per_pixel;
+ int bytes_per_pixel;
+ struct fb_info fb_info;
+ struct fb_var_screeninfo fb_defined;
+ struct fb_fix_screeninfo fb_fix;
+
+ /* Error injection */
+ bool queue_setup_error;
+ bool buf_prepare_error;
+ bool start_streaming_error;
+ bool dqbuf_error;
+ bool req_validate_error;
+ bool seq_wrap;
+ bool time_wrap;
+ u64 time_wrap_offset;
+ unsigned perc_dropped_buffers;
+ enum vivid_signal_mode std_signal_mode[MAX_INPUTS];
+ unsigned int query_std_last[MAX_INPUTS];
+ v4l2_std_id query_std[MAX_INPUTS];
+ enum tpg_video_aspect std_aspect_ratio[MAX_INPUTS];
+
+ enum vivid_signal_mode dv_timings_signal_mode[MAX_INPUTS];
+ char **query_dv_timings_qmenu;
+ char *query_dv_timings_qmenu_strings;
+ unsigned query_dv_timings_size;
+ unsigned int query_dv_timings_last[MAX_INPUTS];
+ unsigned int query_dv_timings[MAX_INPUTS];
+ enum tpg_video_aspect dv_timings_aspect_ratio[MAX_INPUTS];
+
+ /* Input */
+ unsigned input;
+ v4l2_std_id std_cap[MAX_INPUTS];
+ struct v4l2_dv_timings dv_timings_cap[MAX_INPUTS];
+ int dv_timings_cap_sel[MAX_INPUTS];
+ u32 service_set_cap;
+ struct vivid_vbi_gen_data vbi_gen;
+ u8 *edid;
+ unsigned edid_blocks;
+ unsigned edid_max_blocks;
+ unsigned webcam_size_idx;
+ unsigned webcam_ival_idx;
+ unsigned tv_freq;
+ unsigned tv_audmode;
+ unsigned tv_field_cap;
+ unsigned tv_audio_input;
+
+ u32 power_present;
+
+ /* Capture Overlay */
+ struct v4l2_framebuffer fb_cap;
+ struct v4l2_fh *overlay_cap_owner;
+ void *fb_vbase_cap;
+ int overlay_cap_top, overlay_cap_left;
+ enum v4l2_field overlay_cap_field;
+ void *bitmap_cap;
+ struct v4l2_clip clips_cap[MAX_CLIPS];
+ struct v4l2_clip try_clips_cap[MAX_CLIPS];
+ unsigned clipcount_cap;
+
+ /* Output */
+ unsigned output;
+ v4l2_std_id std_out;
+ struct v4l2_dv_timings dv_timings_out;
+ u32 colorspace_out;
+ u32 ycbcr_enc_out;
+ u32 hsv_enc_out;
+ u32 quantization_out;
+ u32 xfer_func_out;
+ u32 service_set_out;
+ unsigned bytesperline_out[TPG_MAX_PLANES];
+ unsigned tv_field_out;
+ unsigned tv_audio_output;
+ bool vbi_out_have_wss;
+ u8 vbi_out_wss[2];
+ bool vbi_out_have_cc[2];
+ u8 vbi_out_cc[2][2];
+ bool dvi_d_out;
+ u8 *scaled_line;
+ u8 *blended_line;
+ unsigned cur_scaled_line;
+ bool display_present[MAX_OUTPUTS];
+
+ /* Output Overlay */
+ void *fb_vbase_out;
+ bool overlay_out_enabled;
+ int overlay_out_top, overlay_out_left;
+ void *bitmap_out;
+ struct v4l2_clip clips_out[MAX_CLIPS];
+ struct v4l2_clip try_clips_out[MAX_CLIPS];
+ unsigned clipcount_out;
+ unsigned fbuf_out_flags;
+ u32 chromakey_out;
+ u8 global_alpha_out;
+
+ /* video capture */
+ struct tpg_data tpg;
+ unsigned ms_vid_cap;
+ bool must_blank[VIDEO_MAX_FRAME];
+
+ const struct vivid_fmt *fmt_cap;
+ struct v4l2_fract timeperframe_vid_cap;
+ enum v4l2_field field_cap;
+ struct v4l2_rect src_rect;
+ struct v4l2_rect fmt_cap_rect;
+ struct v4l2_rect crop_cap;
+ struct v4l2_rect compose_cap;
+ struct v4l2_rect crop_bounds_cap;
+ struct vb2_queue vb_vid_cap_q;
+ struct list_head vid_cap_active;
+ struct vb2_queue vb_vbi_cap_q;
+ struct list_head vbi_cap_active;
+ struct vb2_queue vb_meta_cap_q;
+ struct list_head meta_cap_active;
+ struct vb2_queue vb_touch_cap_q;
+ struct list_head touch_cap_active;
+
+ /* thread for generating video capture stream */
+ struct task_struct *kthread_vid_cap;
+ unsigned long jiffies_vid_cap;
+ u64 cap_stream_start;
+ u64 cap_frame_period;
+ u64 cap_frame_eof_offset;
+ u32 cap_seq_offset;
+ u32 cap_seq_count;
+ bool cap_seq_resync;
+ u32 vid_cap_seq_start;
+ u32 vid_cap_seq_count;
+ bool vid_cap_streaming;
+ u32 vbi_cap_seq_start;
+ u32 vbi_cap_seq_count;
+ bool vbi_cap_streaming;
+ bool stream_sliced_vbi_cap;
+ u32 meta_cap_seq_start;
+ u32 meta_cap_seq_count;
+ bool meta_cap_streaming;
+
+ /* Touch capture */
+ struct task_struct *kthread_touch_cap;
+ unsigned long jiffies_touch_cap;
+ u64 touch_cap_stream_start;
+ u32 touch_cap_seq_offset;
+ bool touch_cap_seq_resync;
+ u32 touch_cap_seq_start;
+ u32 touch_cap_seq_count;
+ bool touch_cap_streaming;
+ struct v4l2_fract timeperframe_tch_cap;
+ struct v4l2_pix_format tch_format;
+ int tch_pat_random;
+
+ /* video output */
+ const struct vivid_fmt *fmt_out;
+ struct v4l2_fract timeperframe_vid_out;
+ enum v4l2_field field_out;
+ struct v4l2_rect sink_rect;
+ struct v4l2_rect fmt_out_rect;
+ struct v4l2_rect crop_out;
+ struct v4l2_rect compose_out;
+ struct v4l2_rect compose_bounds_out;
+ struct vb2_queue vb_vid_out_q;
+ struct list_head vid_out_active;
+ struct vb2_queue vb_vbi_out_q;
+ struct list_head vbi_out_active;
+ struct vb2_queue vb_meta_out_q;
+ struct list_head meta_out_active;
+
+ /* video loop precalculated rectangles */
+
+ /*
+ * Intersection between what the output side composes and the capture side
+ * crops. I.e., what actually needs to be copied from the output buffer to
+ * the capture buffer.
+ */
+ struct v4l2_rect loop_vid_copy;
+ /* The part of the output buffer that (after scaling) corresponds to loop_vid_copy. */
+ struct v4l2_rect loop_vid_out;
+ /* The part of the capture buffer that (after scaling) corresponds to loop_vid_copy. */
+ struct v4l2_rect loop_vid_cap;
+ /*
+ * The intersection of the framebuffer, the overlay output window and
+ * loop_vid_copy. I.e., the part of the framebuffer that actually should be
+ * blended with the compose_out rectangle. This uses the framebuffer origin.
+ */
+ struct v4l2_rect loop_fb_copy;
+ /* The same as loop_fb_copy but with compose_out origin. */
+ struct v4l2_rect loop_vid_overlay;
+ /*
+ * The part of the capture buffer that (after scaling) corresponds
+ * to loop_vid_overlay.
+ */
+ struct v4l2_rect loop_vid_overlay_cap;
+
+ /* thread for generating video output stream */
+ struct task_struct *kthread_vid_out;
+ unsigned long jiffies_vid_out;
+ u32 out_seq_offset;
+ u32 out_seq_count;
+ bool out_seq_resync;
+ u32 vid_out_seq_start;
+ u32 vid_out_seq_count;
+ bool vid_out_streaming;
+ u32 vbi_out_seq_start;
+ u32 vbi_out_seq_count;
+ bool vbi_out_streaming;
+ bool stream_sliced_vbi_out;
+ u32 meta_out_seq_start;
+ u32 meta_out_seq_count;
+ bool meta_out_streaming;
+
+ /* SDR capture */
+ struct vb2_queue vb_sdr_cap_q;
+ struct list_head sdr_cap_active;
+ u32 sdr_pixelformat; /* v4l2 format id */
+ unsigned sdr_buffersize;
+ unsigned sdr_adc_freq;
+ unsigned sdr_fm_freq;
+ unsigned sdr_fm_deviation;
+ int sdr_fixp_src_phase;
+ int sdr_fixp_mod_phase;
+
+ bool tstamp_src_is_soe;
+ bool has_crop_cap;
+ bool has_compose_cap;
+ bool has_scaler_cap;
+ bool has_crop_out;
+ bool has_compose_out;
+ bool has_scaler_out;
+
+ /* thread for generating SDR stream */
+ struct task_struct *kthread_sdr_cap;
+ unsigned long jiffies_sdr_cap;
+ u32 sdr_cap_seq_offset;
+ u32 sdr_cap_seq_count;
+ bool sdr_cap_seq_resync;
+
+ /* RDS generator */
+ struct vivid_rds_gen rds_gen;
+
+ /* Radio receiver */
+ unsigned radio_rx_freq;
+ unsigned radio_rx_audmode;
+ int radio_rx_sig_qual;
+ unsigned radio_rx_hw_seek_mode;
+ bool radio_rx_hw_seek_prog_lim;
+ bool radio_rx_rds_controls;
+ bool radio_rx_rds_enabled;
+ unsigned radio_rx_rds_use_alternates;
+ unsigned radio_rx_rds_last_block;
+ struct v4l2_fh *radio_rx_rds_owner;
+
+ /* Radio transmitter */
+ unsigned radio_tx_freq;
+ unsigned radio_tx_subchans;
+ bool radio_tx_rds_controls;
+ unsigned radio_tx_rds_last_block;
+ struct v4l2_fh *radio_tx_rds_owner;
+
+ /* Shared between radio receiver and transmitter */
+ bool radio_rds_loop;
+ ktime_t radio_rds_init_time;
+
+ /* CEC */
+ struct cec_adapter *cec_rx_adap;
+ struct cec_adapter *cec_tx_adap[MAX_OUTPUTS];
+ struct workqueue_struct *cec_workqueue;
+ spinlock_t cec_slock;
+ struct list_head cec_work_list;
+ unsigned int cec_xfer_time_jiffies;
+ unsigned long cec_xfer_start_jiffies;
+ u8 cec_output2bus_map[MAX_OUTPUTS];
+
+ /* CEC OSD String */
+ char osd[14];
+ unsigned long osd_jiffies;
+
+ bool meta_pts;
+ bool meta_scr;
+};
+
+static inline bool vivid_is_webcam(const struct vivid_dev *dev)
+{
+ return dev->input_type[dev->input] == WEBCAM;
+}
+
+static inline bool vivid_is_tv_cap(const struct vivid_dev *dev)
+{
+ return dev->input_type[dev->input] == TV;
+}
+
+static inline bool vivid_is_svid_cap(const struct vivid_dev *dev)
+{
+ return dev->input_type[dev->input] == SVID;
+}
+
+static inline bool vivid_is_hdmi_cap(const struct vivid_dev *dev)
+{
+ return dev->input_type[dev->input] == HDMI;
+}
+
+static inline bool vivid_is_sdtv_cap(const struct vivid_dev *dev)
+{
+ return vivid_is_tv_cap(dev) || vivid_is_svid_cap(dev);
+}
+
+static inline bool vivid_is_svid_out(const struct vivid_dev *dev)
+{
+ return dev->output_type[dev->output] == SVID;
+}
+
+static inline bool vivid_is_hdmi_out(const struct vivid_dev *dev)
+{
+ return dev->output_type[dev->output] == HDMI;
+}
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-ctrls.c b/drivers/media/test-drivers/vivid/vivid-ctrls.c
new file mode 100644
index 0000000..3341305
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-ctrls.c
@@ -0,0 +1,1939 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-ctrls.c - control support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-vid-common.h"
+#include "vivid-radio-common.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+#include "vivid-cec.h"
+
+#define VIVID_CID_CUSTOM_BASE (V4L2_CID_USER_BASE | 0xf000)
+#define VIVID_CID_BUTTON (VIVID_CID_CUSTOM_BASE + 0)
+#define VIVID_CID_BOOLEAN (VIVID_CID_CUSTOM_BASE + 1)
+#define VIVID_CID_INTEGER (VIVID_CID_CUSTOM_BASE + 2)
+#define VIVID_CID_INTEGER64 (VIVID_CID_CUSTOM_BASE + 3)
+#define VIVID_CID_MENU (VIVID_CID_CUSTOM_BASE + 4)
+#define VIVID_CID_STRING (VIVID_CID_CUSTOM_BASE + 5)
+#define VIVID_CID_BITMASK (VIVID_CID_CUSTOM_BASE + 6)
+#define VIVID_CID_INTMENU (VIVID_CID_CUSTOM_BASE + 7)
+#define VIVID_CID_U32_ARRAY (VIVID_CID_CUSTOM_BASE + 8)
+#define VIVID_CID_U16_MATRIX (VIVID_CID_CUSTOM_BASE + 9)
+#define VIVID_CID_U8_4D_ARRAY (VIVID_CID_CUSTOM_BASE + 10)
+#define VIVID_CID_AREA (VIVID_CID_CUSTOM_BASE + 11)
+
+#define VIVID_CID_VIVID_BASE (0x00f00000 | 0xf000)
+#define VIVID_CID_VIVID_CLASS (0x00f00000 | 1)
+#define VIVID_CID_TEST_PATTERN (VIVID_CID_VIVID_BASE + 0)
+#define VIVID_CID_OSD_TEXT_MODE (VIVID_CID_VIVID_BASE + 1)
+#define VIVID_CID_HOR_MOVEMENT (VIVID_CID_VIVID_BASE + 2)
+#define VIVID_CID_VERT_MOVEMENT (VIVID_CID_VIVID_BASE + 3)
+#define VIVID_CID_SHOW_BORDER (VIVID_CID_VIVID_BASE + 4)
+#define VIVID_CID_SHOW_SQUARE (VIVID_CID_VIVID_BASE + 5)
+#define VIVID_CID_INSERT_SAV (VIVID_CID_VIVID_BASE + 6)
+#define VIVID_CID_INSERT_EAV (VIVID_CID_VIVID_BASE + 7)
+#define VIVID_CID_VBI_CAP_INTERLACED (VIVID_CID_VIVID_BASE + 8)
+
+#define VIVID_CID_HFLIP (VIVID_CID_VIVID_BASE + 20)
+#define VIVID_CID_VFLIP (VIVID_CID_VIVID_BASE + 21)
+#define VIVID_CID_STD_ASPECT_RATIO (VIVID_CID_VIVID_BASE + 22)
+#define VIVID_CID_DV_TIMINGS_ASPECT_RATIO (VIVID_CID_VIVID_BASE + 23)
+#define VIVID_CID_TSTAMP_SRC (VIVID_CID_VIVID_BASE + 24)
+#define VIVID_CID_COLORSPACE (VIVID_CID_VIVID_BASE + 25)
+#define VIVID_CID_XFER_FUNC (VIVID_CID_VIVID_BASE + 26)
+#define VIVID_CID_YCBCR_ENC (VIVID_CID_VIVID_BASE + 27)
+#define VIVID_CID_QUANTIZATION (VIVID_CID_VIVID_BASE + 28)
+#define VIVID_CID_LIMITED_RGB_RANGE (VIVID_CID_VIVID_BASE + 29)
+#define VIVID_CID_ALPHA_MODE (VIVID_CID_VIVID_BASE + 30)
+#define VIVID_CID_HAS_CROP_CAP (VIVID_CID_VIVID_BASE + 31)
+#define VIVID_CID_HAS_COMPOSE_CAP (VIVID_CID_VIVID_BASE + 32)
+#define VIVID_CID_HAS_SCALER_CAP (VIVID_CID_VIVID_BASE + 33)
+#define VIVID_CID_HAS_CROP_OUT (VIVID_CID_VIVID_BASE + 34)
+#define VIVID_CID_HAS_COMPOSE_OUT (VIVID_CID_VIVID_BASE + 35)
+#define VIVID_CID_HAS_SCALER_OUT (VIVID_CID_VIVID_BASE + 36)
+#define VIVID_CID_LOOP_VIDEO (VIVID_CID_VIVID_BASE + 37)
+#define VIVID_CID_SEQ_WRAP (VIVID_CID_VIVID_BASE + 38)
+#define VIVID_CID_TIME_WRAP (VIVID_CID_VIVID_BASE + 39)
+#define VIVID_CID_MAX_EDID_BLOCKS (VIVID_CID_VIVID_BASE + 40)
+#define VIVID_CID_PERCENTAGE_FILL (VIVID_CID_VIVID_BASE + 41)
+#define VIVID_CID_REDUCED_FPS (VIVID_CID_VIVID_BASE + 42)
+#define VIVID_CID_HSV_ENC (VIVID_CID_VIVID_BASE + 43)
+#define VIVID_CID_DISPLAY_PRESENT (VIVID_CID_VIVID_BASE + 44)
+
+#define VIVID_CID_STD_SIGNAL_MODE (VIVID_CID_VIVID_BASE + 60)
+#define VIVID_CID_STANDARD (VIVID_CID_VIVID_BASE + 61)
+#define VIVID_CID_DV_TIMINGS_SIGNAL_MODE (VIVID_CID_VIVID_BASE + 62)
+#define VIVID_CID_DV_TIMINGS (VIVID_CID_VIVID_BASE + 63)
+#define VIVID_CID_PERC_DROPPED (VIVID_CID_VIVID_BASE + 64)
+#define VIVID_CID_DISCONNECT (VIVID_CID_VIVID_BASE + 65)
+#define VIVID_CID_DQBUF_ERROR (VIVID_CID_VIVID_BASE + 66)
+#define VIVID_CID_QUEUE_SETUP_ERROR (VIVID_CID_VIVID_BASE + 67)
+#define VIVID_CID_BUF_PREPARE_ERROR (VIVID_CID_VIVID_BASE + 68)
+#define VIVID_CID_START_STR_ERROR (VIVID_CID_VIVID_BASE + 69)
+#define VIVID_CID_QUEUE_ERROR (VIVID_CID_VIVID_BASE + 70)
+#define VIVID_CID_CLEAR_FB (VIVID_CID_VIVID_BASE + 71)
+#define VIVID_CID_REQ_VALIDATE_ERROR (VIVID_CID_VIVID_BASE + 72)
+
+#define VIVID_CID_RADIO_SEEK_MODE (VIVID_CID_VIVID_BASE + 90)
+#define VIVID_CID_RADIO_SEEK_PROG_LIM (VIVID_CID_VIVID_BASE + 91)
+#define VIVID_CID_RADIO_RX_RDS_RBDS (VIVID_CID_VIVID_BASE + 92)
+#define VIVID_CID_RADIO_RX_RDS_BLOCKIO (VIVID_CID_VIVID_BASE + 93)
+
+#define VIVID_CID_RADIO_TX_RDS_BLOCKIO (VIVID_CID_VIVID_BASE + 94)
+
+#define VIVID_CID_SDR_CAP_FM_DEVIATION (VIVID_CID_VIVID_BASE + 110)
+
+#define VIVID_CID_META_CAP_GENERATE_PTS (VIVID_CID_VIVID_BASE + 111)
+#define VIVID_CID_META_CAP_GENERATE_SCR (VIVID_CID_VIVID_BASE + 112)
+
+/* General User Controls */
+
+static int vivid_user_gen_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_user_gen);
+
+ switch (ctrl->id) {
+ case VIVID_CID_DISCONNECT:
+ v4l2_info(&dev->v4l2_dev, "disconnect\n");
+ clear_bit(V4L2_FL_REGISTERED, &dev->vid_cap_dev.flags);
+ clear_bit(V4L2_FL_REGISTERED, &dev->vid_out_dev.flags);
+ clear_bit(V4L2_FL_REGISTERED, &dev->vbi_cap_dev.flags);
+ clear_bit(V4L2_FL_REGISTERED, &dev->vbi_out_dev.flags);
+ clear_bit(V4L2_FL_REGISTERED, &dev->sdr_cap_dev.flags);
+ clear_bit(V4L2_FL_REGISTERED, &dev->radio_rx_dev.flags);
+ clear_bit(V4L2_FL_REGISTERED, &dev->radio_tx_dev.flags);
+ clear_bit(V4L2_FL_REGISTERED, &dev->meta_cap_dev.flags);
+ break;
+ case VIVID_CID_BUTTON:
+ dev->button_pressed = 30;
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_user_gen_ctrl_ops = {
+ .s_ctrl = vivid_user_gen_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_button = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_BUTTON,
+ .name = "Button",
+ .type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_boolean = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_BOOLEAN,
+ .name = "Boolean",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .min = 0,
+ .max = 1,
+ .step = 1,
+ .def = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_int32 = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_INTEGER,
+ .name = "Integer 32 Bits",
+ .type = V4L2_CTRL_TYPE_INTEGER,
+ .min = 0xffffffff80000000ULL,
+ .max = 0x7fffffff,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_int64 = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_INTEGER64,
+ .name = "Integer 64 Bits",
+ .type = V4L2_CTRL_TYPE_INTEGER64,
+ .min = 0x8000000000000000ULL,
+ .max = 0x7fffffffffffffffLL,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_u32_array = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_U32_ARRAY,
+ .name = "U32 1 Element Array",
+ .type = V4L2_CTRL_TYPE_U32,
+ .def = 0x18,
+ .min = 0x10,
+ .max = 0x20000,
+ .step = 1,
+ .dims = { 1 },
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_u16_matrix = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_U16_MATRIX,
+ .name = "U16 8x16 Matrix",
+ .type = V4L2_CTRL_TYPE_U16,
+ .def = 0x18,
+ .min = 0x10,
+ .max = 0x2000,
+ .step = 1,
+ .dims = { 8, 16 },
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_u8_4d_array = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_U8_4D_ARRAY,
+ .name = "U8 2x3x4x5 Array",
+ .type = V4L2_CTRL_TYPE_U8,
+ .def = 0x18,
+ .min = 0x10,
+ .max = 0x20,
+ .step = 1,
+ .dims = { 2, 3, 4, 5 },
+};
+
+static const char * const vivid_ctrl_menu_strings[] = {
+ "Menu Item 0 (Skipped)",
+ "Menu Item 1",
+ "Menu Item 2 (Skipped)",
+ "Menu Item 3",
+ "Menu Item 4",
+ "Menu Item 5 (Skipped)",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_menu = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_MENU,
+ .name = "Menu",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .min = 1,
+ .max = 4,
+ .def = 3,
+ .menu_skip_mask = 0x04,
+ .qmenu = vivid_ctrl_menu_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_string = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_STRING,
+ .name = "String",
+ .type = V4L2_CTRL_TYPE_STRING,
+ .min = 2,
+ .max = 4,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_bitmask = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_BITMASK,
+ .name = "Bitmask",
+ .type = V4L2_CTRL_TYPE_BITMASK,
+ .def = 0x80002000,
+ .min = 0,
+ .max = 0x80402010,
+ .step = 0,
+};
+
+static const s64 vivid_ctrl_int_menu_values[] = {
+ 1, 1, 2, 3, 5, 8, 13, 21, 42,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_int_menu = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_INTMENU,
+ .name = "Integer Menu",
+ .type = V4L2_CTRL_TYPE_INTEGER_MENU,
+ .min = 1,
+ .max = 8,
+ .def = 4,
+ .menu_skip_mask = 0x02,
+ .qmenu_int = vivid_ctrl_int_menu_values,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_disconnect = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_DISCONNECT,
+ .name = "Disconnect",
+ .type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_area area = {
+ .width = 1000,
+ .height = 2000,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_area = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .id = VIVID_CID_AREA,
+ .name = "Area",
+ .type = V4L2_CTRL_TYPE_AREA,
+ .p_def.p_const = &area,
+};
+
+/* Framebuffer Controls */
+
+static int vivid_fb_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler,
+ struct vivid_dev, ctrl_hdl_fb);
+
+ switch (ctrl->id) {
+ case VIVID_CID_CLEAR_FB:
+ vivid_clear_fb(dev);
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_fb_ctrl_ops = {
+ .s_ctrl = vivid_fb_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_clear_fb = {
+ .ops = &vivid_fb_ctrl_ops,
+ .id = VIVID_CID_CLEAR_FB,
+ .name = "Clear Framebuffer",
+ .type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+
+/* Video User Controls */
+
+static int vivid_user_vid_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_user_vid);
+
+ switch (ctrl->id) {
+ case V4L2_CID_AUTOGAIN:
+ dev->gain->val = (jiffies_to_msecs(jiffies) / 1000) & 0xff;
+ break;
+ }
+ return 0;
+}
+
+static int vivid_user_vid_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_user_vid);
+
+ switch (ctrl->id) {
+ case V4L2_CID_BRIGHTNESS:
+ dev->input_brightness[dev->input] = ctrl->val - dev->input * 128;
+ tpg_s_brightness(&dev->tpg, dev->input_brightness[dev->input]);
+ break;
+ case V4L2_CID_CONTRAST:
+ tpg_s_contrast(&dev->tpg, ctrl->val);
+ break;
+ case V4L2_CID_SATURATION:
+ tpg_s_saturation(&dev->tpg, ctrl->val);
+ break;
+ case V4L2_CID_HUE:
+ tpg_s_hue(&dev->tpg, ctrl->val);
+ break;
+ case V4L2_CID_HFLIP:
+ dev->hflip = ctrl->val;
+ tpg_s_hflip(&dev->tpg, dev->sensor_hflip ^ dev->hflip);
+ break;
+ case V4L2_CID_VFLIP:
+ dev->vflip = ctrl->val;
+ tpg_s_vflip(&dev->tpg, dev->sensor_vflip ^ dev->vflip);
+ break;
+ case V4L2_CID_ALPHA_COMPONENT:
+ tpg_s_alpha_component(&dev->tpg, ctrl->val);
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_user_vid_ctrl_ops = {
+ .g_volatile_ctrl = vivid_user_vid_g_volatile_ctrl,
+ .s_ctrl = vivid_user_vid_s_ctrl,
+};
+
+
+/* Video Capture Controls */
+
+static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ static const u32 colorspaces[] = {
+ V4L2_COLORSPACE_SMPTE170M,
+ V4L2_COLORSPACE_REC709,
+ V4L2_COLORSPACE_SRGB,
+ V4L2_COLORSPACE_OPRGB,
+ V4L2_COLORSPACE_BT2020,
+ V4L2_COLORSPACE_DCI_P3,
+ V4L2_COLORSPACE_SMPTE240M,
+ V4L2_COLORSPACE_470_SYSTEM_M,
+ V4L2_COLORSPACE_470_SYSTEM_BG,
+ };
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vid_cap);
+ unsigned int i, j;
+
+ switch (ctrl->id) {
+ case VIVID_CID_TEST_PATTERN:
+ vivid_update_quality(dev);
+ tpg_s_pattern(&dev->tpg, ctrl->val);
+ break;
+ case VIVID_CID_COLORSPACE:
+ tpg_s_colorspace(&dev->tpg, colorspaces[ctrl->val]);
+ vivid_send_source_change(dev, TV);
+ vivid_send_source_change(dev, SVID);
+ vivid_send_source_change(dev, HDMI);
+ vivid_send_source_change(dev, WEBCAM);
+ break;
+ case VIVID_CID_XFER_FUNC:
+ tpg_s_xfer_func(&dev->tpg, ctrl->val);
+ vivid_send_source_change(dev, TV);
+ vivid_send_source_change(dev, SVID);
+ vivid_send_source_change(dev, HDMI);
+ vivid_send_source_change(dev, WEBCAM);
+ break;
+ case VIVID_CID_YCBCR_ENC:
+ tpg_s_ycbcr_enc(&dev->tpg, ctrl->val);
+ vivid_send_source_change(dev, TV);
+ vivid_send_source_change(dev, SVID);
+ vivid_send_source_change(dev, HDMI);
+ vivid_send_source_change(dev, WEBCAM);
+ break;
+ case VIVID_CID_HSV_ENC:
+ tpg_s_hsv_enc(&dev->tpg, ctrl->val ? V4L2_HSV_ENC_256 :
+ V4L2_HSV_ENC_180);
+ vivid_send_source_change(dev, TV);
+ vivid_send_source_change(dev, SVID);
+ vivid_send_source_change(dev, HDMI);
+ vivid_send_source_change(dev, WEBCAM);
+ break;
+ case VIVID_CID_QUANTIZATION:
+ tpg_s_quantization(&dev->tpg, ctrl->val);
+ vivid_send_source_change(dev, TV);
+ vivid_send_source_change(dev, SVID);
+ vivid_send_source_change(dev, HDMI);
+ vivid_send_source_change(dev, WEBCAM);
+ break;
+ case V4L2_CID_DV_RX_RGB_RANGE:
+ if (!vivid_is_hdmi_cap(dev))
+ break;
+ tpg_s_rgb_range(&dev->tpg, ctrl->val);
+ break;
+ case VIVID_CID_LIMITED_RGB_RANGE:
+ tpg_s_real_rgb_range(&dev->tpg, ctrl->val ?
+ V4L2_DV_RGB_RANGE_LIMITED : V4L2_DV_RGB_RANGE_FULL);
+ break;
+ case VIVID_CID_ALPHA_MODE:
+ tpg_s_alpha_mode(&dev->tpg, ctrl->val);
+ break;
+ case VIVID_CID_HOR_MOVEMENT:
+ tpg_s_mv_hor_mode(&dev->tpg, ctrl->val);
+ break;
+ case VIVID_CID_VERT_MOVEMENT:
+ tpg_s_mv_vert_mode(&dev->tpg, ctrl->val);
+ break;
+ case VIVID_CID_OSD_TEXT_MODE:
+ dev->osd_mode = ctrl->val;
+ break;
+ case VIVID_CID_PERCENTAGE_FILL:
+ tpg_s_perc_fill(&dev->tpg, ctrl->val);
+ for (i = 0; i < VIDEO_MAX_FRAME; i++)
+ dev->must_blank[i] = ctrl->val < 100;
+ break;
+ case VIVID_CID_INSERT_SAV:
+ tpg_s_insert_sav(&dev->tpg, ctrl->val);
+ break;
+ case VIVID_CID_INSERT_EAV:
+ tpg_s_insert_eav(&dev->tpg, ctrl->val);
+ break;
+ case VIVID_CID_HFLIP:
+ dev->sensor_hflip = ctrl->val;
+ tpg_s_hflip(&dev->tpg, dev->sensor_hflip ^ dev->hflip);
+ break;
+ case VIVID_CID_VFLIP:
+ dev->sensor_vflip = ctrl->val;
+ tpg_s_vflip(&dev->tpg, dev->sensor_vflip ^ dev->vflip);
+ break;
+ case VIVID_CID_REDUCED_FPS:
+ dev->reduced_fps = ctrl->val;
+ vivid_update_format_cap(dev, true);
+ break;
+ case VIVID_CID_HAS_CROP_CAP:
+ dev->has_crop_cap = ctrl->val;
+ vivid_update_format_cap(dev, true);
+ break;
+ case VIVID_CID_HAS_COMPOSE_CAP:
+ dev->has_compose_cap = ctrl->val;
+ vivid_update_format_cap(dev, true);
+ break;
+ case VIVID_CID_HAS_SCALER_CAP:
+ dev->has_scaler_cap = ctrl->val;
+ vivid_update_format_cap(dev, true);
+ break;
+ case VIVID_CID_SHOW_BORDER:
+ tpg_s_show_border(&dev->tpg, ctrl->val);
+ break;
+ case VIVID_CID_SHOW_SQUARE:
+ tpg_s_show_square(&dev->tpg, ctrl->val);
+ break;
+ case VIVID_CID_STD_ASPECT_RATIO:
+ dev->std_aspect_ratio[dev->input] = ctrl->val;
+ tpg_s_video_aspect(&dev->tpg, vivid_get_video_aspect(dev));
+ break;
+ case VIVID_CID_DV_TIMINGS_SIGNAL_MODE:
+ dev->dv_timings_signal_mode[dev->input] =
+ dev->ctrl_dv_timings_signal_mode->val;
+ dev->query_dv_timings[dev->input] = dev->ctrl_dv_timings->val;
+
+ dev->power_present = 0;
+ for (i = 0, j = 0;
+ i < ARRAY_SIZE(dev->dv_timings_signal_mode);
+ i++)
+ if (dev->input_type[i] == HDMI) {
+ if (dev->dv_timings_signal_mode[i] != NO_SIGNAL)
+ dev->power_present |= (1 << j);
+ j++;
+ }
+ __v4l2_ctrl_s_ctrl(dev->ctrl_rx_power_present,
+ dev->power_present);
+
+ v4l2_ctrl_activate(dev->ctrl_dv_timings,
+ dev->dv_timings_signal_mode[dev->input] ==
+ SELECTED_DV_TIMINGS);
+
+ vivid_update_quality(dev);
+ vivid_send_source_change(dev, HDMI);
+ break;
+ case VIVID_CID_DV_TIMINGS_ASPECT_RATIO:
+ dev->dv_timings_aspect_ratio[dev->input] = ctrl->val;
+ tpg_s_video_aspect(&dev->tpg, vivid_get_video_aspect(dev));
+ break;
+ case VIVID_CID_TSTAMP_SRC:
+ dev->tstamp_src_is_soe = ctrl->val;
+ dev->vb_vid_cap_q.timestamp_flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+ if (dev->tstamp_src_is_soe)
+ dev->vb_vid_cap_q.timestamp_flags |= V4L2_BUF_FLAG_TSTAMP_SRC_SOE;
+ break;
+ case VIVID_CID_MAX_EDID_BLOCKS:
+ dev->edid_max_blocks = ctrl->val;
+ if (dev->edid_blocks > dev->edid_max_blocks)
+ dev->edid_blocks = dev->edid_max_blocks;
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_vid_cap_ctrl_ops = {
+ .s_ctrl = vivid_vid_cap_s_ctrl,
+};
+
+static const char * const vivid_ctrl_hor_movement_strings[] = {
+ "Move Left Fast",
+ "Move Left",
+ "Move Left Slow",
+ "No Movement",
+ "Move Right Slow",
+ "Move Right",
+ "Move Right Fast",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_hor_movement = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_HOR_MOVEMENT,
+ .name = "Horizontal Movement",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = TPG_MOVE_POS_FAST,
+ .def = TPG_MOVE_NONE,
+ .qmenu = vivid_ctrl_hor_movement_strings,
+};
+
+static const char * const vivid_ctrl_vert_movement_strings[] = {
+ "Move Up Fast",
+ "Move Up",
+ "Move Up Slow",
+ "No Movement",
+ "Move Down Slow",
+ "Move Down",
+ "Move Down Fast",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_vert_movement = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_VERT_MOVEMENT,
+ .name = "Vertical Movement",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = TPG_MOVE_POS_FAST,
+ .def = TPG_MOVE_NONE,
+ .qmenu = vivid_ctrl_vert_movement_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_show_border = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_SHOW_BORDER,
+ .name = "Show Border",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_show_square = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_SHOW_SQUARE,
+ .name = "Show Square",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const char * const vivid_ctrl_osd_mode_strings[] = {
+ "All",
+ "Counters Only",
+ "None",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_osd_mode = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_OSD_TEXT_MODE,
+ .name = "OSD Text Mode",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = ARRAY_SIZE(vivid_ctrl_osd_mode_strings) - 2,
+ .qmenu = vivid_ctrl_osd_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_perc_fill = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_PERCENTAGE_FILL,
+ .name = "Fill Percentage of Frame",
+ .type = V4L2_CTRL_TYPE_INTEGER,
+ .min = 0,
+ .max = 100,
+ .def = 100,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_insert_sav = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_INSERT_SAV,
+ .name = "Insert SAV Code in Image",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_insert_eav = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_INSERT_EAV,
+ .name = "Insert EAV Code in Image",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_hflip = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_HFLIP,
+ .name = "Sensor Flipped Horizontally",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_vflip = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_VFLIP,
+ .name = "Sensor Flipped Vertically",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_reduced_fps = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_REDUCED_FPS,
+ .name = "Reduced Framerate",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_crop_cap = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_HAS_CROP_CAP,
+ .name = "Enable Capture Cropping",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .def = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_compose_cap = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_HAS_COMPOSE_CAP,
+ .name = "Enable Capture Composing",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .def = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_scaler_cap = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_HAS_SCALER_CAP,
+ .name = "Enable Capture Scaler",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .def = 1,
+ .step = 1,
+};
+
+static const char * const vivid_ctrl_tstamp_src_strings[] = {
+ "End of Frame",
+ "Start of Exposure",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_tstamp_src = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_TSTAMP_SRC,
+ .name = "Timestamp Source",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = ARRAY_SIZE(vivid_ctrl_tstamp_src_strings) - 2,
+ .qmenu = vivid_ctrl_tstamp_src_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_std_aspect_ratio = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_STD_ASPECT_RATIO,
+ .name = "Standard Aspect Ratio",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .min = 1,
+ .max = 4,
+ .def = 1,
+ .qmenu = tpg_aspect_strings,
+};
+
+static const char * const vivid_ctrl_dv_timings_signal_mode_strings[] = {
+ "Current DV Timings",
+ "No Signal",
+ "No Lock",
+ "Out of Range",
+ "Selected DV Timings",
+ "Cycle Through All DV Timings",
+ "Custom DV Timings",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_dv_timings_signal_mode = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_DV_TIMINGS_SIGNAL_MODE,
+ .name = "DV Timings Signal Mode",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = 5,
+ .qmenu = vivid_ctrl_dv_timings_signal_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_dv_timings_aspect_ratio = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_DV_TIMINGS_ASPECT_RATIO,
+ .name = "DV Timings Aspect Ratio",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = 3,
+ .qmenu = tpg_aspect_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_max_edid_blocks = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_MAX_EDID_BLOCKS,
+ .name = "Maximum EDID Blocks",
+ .type = V4L2_CTRL_TYPE_INTEGER,
+ .min = 1,
+ .max = 256,
+ .def = 2,
+ .step = 1,
+};
+
+static const char * const vivid_ctrl_colorspace_strings[] = {
+ "SMPTE 170M",
+ "Rec. 709",
+ "sRGB",
+ "opRGB",
+ "BT.2020",
+ "DCI-P3",
+ "SMPTE 240M",
+ "470 System M",
+ "470 System BG",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_colorspace = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_COLORSPACE,
+ .name = "Colorspace",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = ARRAY_SIZE(vivid_ctrl_colorspace_strings) - 2,
+ .def = 2,
+ .qmenu = vivid_ctrl_colorspace_strings,
+};
+
+static const char * const vivid_ctrl_xfer_func_strings[] = {
+ "Default",
+ "Rec. 709",
+ "sRGB",
+ "opRGB",
+ "SMPTE 240M",
+ "None",
+ "DCI-P3",
+ "SMPTE 2084",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_xfer_func = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_XFER_FUNC,
+ .name = "Transfer Function",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = ARRAY_SIZE(vivid_ctrl_xfer_func_strings) - 2,
+ .qmenu = vivid_ctrl_xfer_func_strings,
+};
+
+static const char * const vivid_ctrl_ycbcr_enc_strings[] = {
+ "Default",
+ "ITU-R 601",
+ "Rec. 709",
+ "xvYCC 601",
+ "xvYCC 709",
+ "",
+ "BT.2020",
+ "BT.2020 Constant Luminance",
+ "SMPTE 240M",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_ycbcr_enc = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_YCBCR_ENC,
+ .name = "Y'CbCr Encoding",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .menu_skip_mask = 1 << 5,
+ .max = ARRAY_SIZE(vivid_ctrl_ycbcr_enc_strings) - 2,
+ .qmenu = vivid_ctrl_ycbcr_enc_strings,
+};
+
+static const char * const vivid_ctrl_hsv_enc_strings[] = {
+ "Hue 0-179",
+ "Hue 0-256",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_hsv_enc = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_HSV_ENC,
+ .name = "HSV Encoding",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = ARRAY_SIZE(vivid_ctrl_hsv_enc_strings) - 2,
+ .qmenu = vivid_ctrl_hsv_enc_strings,
+};
+
+static const char * const vivid_ctrl_quantization_strings[] = {
+ "Default",
+ "Full Range",
+ "Limited Range",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_quantization = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_QUANTIZATION,
+ .name = "Quantization",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = ARRAY_SIZE(vivid_ctrl_quantization_strings) - 2,
+ .qmenu = vivid_ctrl_quantization_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_alpha_mode = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_ALPHA_MODE,
+ .name = "Apply Alpha To Red Only",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_limited_rgb_range = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_LIMITED_RGB_RANGE,
+ .name = "Limited RGB Range (16-235)",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+
+/* Video Loop Control */
+
+static int vivid_loop_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_loop_cap);
+
+ switch (ctrl->id) {
+ case VIVID_CID_LOOP_VIDEO:
+ dev->loop_video = ctrl->val;
+ vivid_update_quality(dev);
+ vivid_send_source_change(dev, SVID);
+ vivid_send_source_change(dev, HDMI);
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_loop_cap_ctrl_ops = {
+ .s_ctrl = vivid_loop_cap_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_loop_video = {
+ .ops = &vivid_loop_cap_ctrl_ops,
+ .id = VIVID_CID_LOOP_VIDEO,
+ .name = "Loop Video",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+
+/* VBI Capture Control */
+
+static int vivid_vbi_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vbi_cap);
+
+ switch (ctrl->id) {
+ case VIVID_CID_VBI_CAP_INTERLACED:
+ dev->vbi_cap_interlaced = ctrl->val;
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_vbi_cap_ctrl_ops = {
+ .s_ctrl = vivid_vbi_cap_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_vbi_cap_interlaced = {
+ .ops = &vivid_vbi_cap_ctrl_ops,
+ .id = VIVID_CID_VBI_CAP_INTERLACED,
+ .name = "Interlaced VBI Format",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+
+/* Video Output Controls */
+
+static int vivid_vid_out_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vid_out);
+ struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
+ u32 display_present = 0;
+ unsigned int i, j, bus_idx;
+
+ switch (ctrl->id) {
+ case VIVID_CID_HAS_CROP_OUT:
+ dev->has_crop_out = ctrl->val;
+ vivid_update_format_out(dev);
+ break;
+ case VIVID_CID_HAS_COMPOSE_OUT:
+ dev->has_compose_out = ctrl->val;
+ vivid_update_format_out(dev);
+ break;
+ case VIVID_CID_HAS_SCALER_OUT:
+ dev->has_scaler_out = ctrl->val;
+ vivid_update_format_out(dev);
+ break;
+ case V4L2_CID_DV_TX_MODE:
+ dev->dvi_d_out = ctrl->val == V4L2_DV_TX_MODE_DVI_D;
+ if (!vivid_is_hdmi_out(dev))
+ break;
+ if (!dev->dvi_d_out && (bt->flags & V4L2_DV_FL_IS_CE_VIDEO)) {
+ if (bt->width == 720 && bt->height <= 576)
+ dev->colorspace_out = V4L2_COLORSPACE_SMPTE170M;
+ else
+ dev->colorspace_out = V4L2_COLORSPACE_REC709;
+ dev->quantization_out = V4L2_QUANTIZATION_DEFAULT;
+ } else {
+ dev->colorspace_out = V4L2_COLORSPACE_SRGB;
+ dev->quantization_out = dev->dvi_d_out ?
+ V4L2_QUANTIZATION_LIM_RANGE :
+ V4L2_QUANTIZATION_DEFAULT;
+ }
+ if (dev->loop_video)
+ vivid_send_source_change(dev, HDMI);
+ break;
+ case VIVID_CID_DISPLAY_PRESENT:
+ if (dev->output_type[dev->output] != HDMI)
+ break;
+
+ dev->display_present[dev->output] = ctrl->val;
+ for (i = 0, j = 0; i < dev->num_outputs; i++)
+ if (dev->output_type[i] == HDMI)
+ display_present |=
+ dev->display_present[i] << j++;
+
+ __v4l2_ctrl_s_ctrl(dev->ctrl_tx_rxsense, display_present);
+
+ if (dev->edid_blocks) {
+ __v4l2_ctrl_s_ctrl(dev->ctrl_tx_edid_present,
+ display_present);
+ __v4l2_ctrl_s_ctrl(dev->ctrl_tx_hotplug,
+ display_present);
+ }
+
+ bus_idx = dev->cec_output2bus_map[dev->output];
+ if (!dev->cec_tx_adap[bus_idx])
+ break;
+
+ if (ctrl->val && dev->edid_blocks)
+ cec_s_phys_addr(dev->cec_tx_adap[bus_idx],
+ dev->cec_tx_adap[bus_idx]->phys_addr,
+ false);
+ else
+ cec_phys_addr_invalidate(dev->cec_tx_adap[bus_idx]);
+
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_vid_out_ctrl_ops = {
+ .s_ctrl = vivid_vid_out_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_crop_out = {
+ .ops = &vivid_vid_out_ctrl_ops,
+ .id = VIVID_CID_HAS_CROP_OUT,
+ .name = "Enable Output Cropping",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .def = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_compose_out = {
+ .ops = &vivid_vid_out_ctrl_ops,
+ .id = VIVID_CID_HAS_COMPOSE_OUT,
+ .name = "Enable Output Composing",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .def = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_scaler_out = {
+ .ops = &vivid_vid_out_ctrl_ops,
+ .id = VIVID_CID_HAS_SCALER_OUT,
+ .name = "Enable Output Scaler",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .def = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_display_present = {
+ .ops = &vivid_vid_out_ctrl_ops,
+ .id = VIVID_CID_DISPLAY_PRESENT,
+ .name = "Display Present",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .def = 1,
+ .step = 1,
+};
+
+/* Streaming Controls */
+
+static int vivid_streaming_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_streaming);
+ u64 rem;
+
+ switch (ctrl->id) {
+ case VIVID_CID_DQBUF_ERROR:
+ dev->dqbuf_error = true;
+ break;
+ case VIVID_CID_PERC_DROPPED:
+ dev->perc_dropped_buffers = ctrl->val;
+ break;
+ case VIVID_CID_QUEUE_SETUP_ERROR:
+ dev->queue_setup_error = true;
+ break;
+ case VIVID_CID_BUF_PREPARE_ERROR:
+ dev->buf_prepare_error = true;
+ break;
+ case VIVID_CID_START_STR_ERROR:
+ dev->start_streaming_error = true;
+ break;
+ case VIVID_CID_REQ_VALIDATE_ERROR:
+ dev->req_validate_error = true;
+ break;
+ case VIVID_CID_QUEUE_ERROR:
+ if (vb2_start_streaming_called(&dev->vb_vid_cap_q))
+ vb2_queue_error(&dev->vb_vid_cap_q);
+ if (vb2_start_streaming_called(&dev->vb_vbi_cap_q))
+ vb2_queue_error(&dev->vb_vbi_cap_q);
+ if (vb2_start_streaming_called(&dev->vb_vid_out_q))
+ vb2_queue_error(&dev->vb_vid_out_q);
+ if (vb2_start_streaming_called(&dev->vb_vbi_out_q))
+ vb2_queue_error(&dev->vb_vbi_out_q);
+ if (vb2_start_streaming_called(&dev->vb_sdr_cap_q))
+ vb2_queue_error(&dev->vb_sdr_cap_q);
+ break;
+ case VIVID_CID_SEQ_WRAP:
+ dev->seq_wrap = ctrl->val;
+ break;
+ case VIVID_CID_TIME_WRAP:
+ dev->time_wrap = ctrl->val;
+ if (ctrl->val == 0) {
+ dev->time_wrap_offset = 0;
+ break;
+ }
+ /*
+ * We want to set the time 16 seconds before the 32 bit tv_sec
+ * value of struct timeval would wrap around. So first we
+ * calculate ktime_get_ns() % ((1 << 32) * NSEC_PER_SEC), and
+ * then we set the offset to ((1 << 32) - 16) * NSEC_PER_SEC).
+ */
+ div64_u64_rem(ktime_get_ns(),
+ 0x100000000ULL * NSEC_PER_SEC, &rem);
+ dev->time_wrap_offset =
+ (0x100000000ULL - 16) * NSEC_PER_SEC - rem;
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_streaming_ctrl_ops = {
+ .s_ctrl = vivid_streaming_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_dqbuf_error = {
+ .ops = &vivid_streaming_ctrl_ops,
+ .id = VIVID_CID_DQBUF_ERROR,
+ .name = "Inject V4L2_BUF_FLAG_ERROR",
+ .type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_perc_dropped = {
+ .ops = &vivid_streaming_ctrl_ops,
+ .id = VIVID_CID_PERC_DROPPED,
+ .name = "Percentage of Dropped Buffers",
+ .type = V4L2_CTRL_TYPE_INTEGER,
+ .min = 0,
+ .max = 100,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_queue_setup_error = {
+ .ops = &vivid_streaming_ctrl_ops,
+ .id = VIVID_CID_QUEUE_SETUP_ERROR,
+ .name = "Inject VIDIOC_REQBUFS Error",
+ .type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_buf_prepare_error = {
+ .ops = &vivid_streaming_ctrl_ops,
+ .id = VIVID_CID_BUF_PREPARE_ERROR,
+ .name = "Inject VIDIOC_QBUF Error",
+ .type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_start_streaming_error = {
+ .ops = &vivid_streaming_ctrl_ops,
+ .id = VIVID_CID_START_STR_ERROR,
+ .name = "Inject VIDIOC_STREAMON Error",
+ .type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_queue_error = {
+ .ops = &vivid_streaming_ctrl_ops,
+ .id = VIVID_CID_QUEUE_ERROR,
+ .name = "Inject Fatal Streaming Error",
+ .type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+#ifdef CONFIG_MEDIA_CONTROLLER
+static const struct v4l2_ctrl_config vivid_ctrl_req_validate_error = {
+ .ops = &vivid_streaming_ctrl_ops,
+ .id = VIVID_CID_REQ_VALIDATE_ERROR,
+ .name = "Inject req_validate() Error",
+ .type = V4L2_CTRL_TYPE_BUTTON,
+};
+#endif
+
+static const struct v4l2_ctrl_config vivid_ctrl_seq_wrap = {
+ .ops = &vivid_streaming_ctrl_ops,
+ .id = VIVID_CID_SEQ_WRAP,
+ .name = "Wrap Sequence Number",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_time_wrap = {
+ .ops = &vivid_streaming_ctrl_ops,
+ .id = VIVID_CID_TIME_WRAP,
+ .name = "Wrap Timestamp",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+
+/* SDTV Capture Controls */
+
+static int vivid_sdtv_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_sdtv_cap);
+
+ switch (ctrl->id) {
+ case VIVID_CID_STD_SIGNAL_MODE:
+ dev->std_signal_mode[dev->input] =
+ dev->ctrl_std_signal_mode->val;
+ if (dev->std_signal_mode[dev->input] == SELECTED_STD)
+ dev->query_std[dev->input] =
+ vivid_standard[dev->ctrl_standard->val];
+ v4l2_ctrl_activate(dev->ctrl_standard,
+ dev->std_signal_mode[dev->input] ==
+ SELECTED_STD);
+ vivid_update_quality(dev);
+ vivid_send_source_change(dev, TV);
+ vivid_send_source_change(dev, SVID);
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_sdtv_cap_ctrl_ops = {
+ .s_ctrl = vivid_sdtv_cap_s_ctrl,
+};
+
+static const char * const vivid_ctrl_std_signal_mode_strings[] = {
+ "Current Standard",
+ "No Signal",
+ "No Lock",
+ "",
+ "Selected Standard",
+ "Cycle Through All Standards",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_std_signal_mode = {
+ .ops = &vivid_sdtv_cap_ctrl_ops,
+ .id = VIVID_CID_STD_SIGNAL_MODE,
+ .name = "Standard Signal Mode",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = ARRAY_SIZE(vivid_ctrl_std_signal_mode_strings) - 2,
+ .menu_skip_mask = 1 << 3,
+ .qmenu = vivid_ctrl_std_signal_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_standard = {
+ .ops = &vivid_sdtv_cap_ctrl_ops,
+ .id = VIVID_CID_STANDARD,
+ .name = "Standard",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = 14,
+ .qmenu = vivid_ctrl_standard_strings,
+};
+
+
+
+/* Radio Receiver Controls */
+
+static int vivid_radio_rx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_radio_rx);
+
+ switch (ctrl->id) {
+ case VIVID_CID_RADIO_SEEK_MODE:
+ dev->radio_rx_hw_seek_mode = ctrl->val;
+ break;
+ case VIVID_CID_RADIO_SEEK_PROG_LIM:
+ dev->radio_rx_hw_seek_prog_lim = ctrl->val;
+ break;
+ case VIVID_CID_RADIO_RX_RDS_RBDS:
+ dev->rds_gen.use_rbds = ctrl->val;
+ break;
+ case VIVID_CID_RADIO_RX_RDS_BLOCKIO:
+ dev->radio_rx_rds_controls = ctrl->val;
+ dev->radio_rx_caps &= ~V4L2_CAP_READWRITE;
+ dev->radio_rx_rds_use_alternates = false;
+ if (!dev->radio_rx_rds_controls) {
+ dev->radio_rx_caps |= V4L2_CAP_READWRITE;
+ __v4l2_ctrl_s_ctrl(dev->radio_rx_rds_pty, 0);
+ __v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ta, 0);
+ __v4l2_ctrl_s_ctrl(dev->radio_rx_rds_tp, 0);
+ __v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ms, 0);
+ __v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_psname, "");
+ __v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_radiotext, "");
+ }
+ v4l2_ctrl_activate(dev->radio_rx_rds_pty, dev->radio_rx_rds_controls);
+ v4l2_ctrl_activate(dev->radio_rx_rds_psname, dev->radio_rx_rds_controls);
+ v4l2_ctrl_activate(dev->radio_rx_rds_radiotext, dev->radio_rx_rds_controls);
+ v4l2_ctrl_activate(dev->radio_rx_rds_ta, dev->radio_rx_rds_controls);
+ v4l2_ctrl_activate(dev->radio_rx_rds_tp, dev->radio_rx_rds_controls);
+ v4l2_ctrl_activate(dev->radio_rx_rds_ms, dev->radio_rx_rds_controls);
+ dev->radio_rx_dev.device_caps = dev->radio_rx_caps;
+ break;
+ case V4L2_CID_RDS_RECEPTION:
+ dev->radio_rx_rds_enabled = ctrl->val;
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_radio_rx_ctrl_ops = {
+ .s_ctrl = vivid_radio_rx_s_ctrl,
+};
+
+static const char * const vivid_ctrl_radio_rds_mode_strings[] = {
+ "Block I/O",
+ "Controls",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_rx_rds_blockio = {
+ .ops = &vivid_radio_rx_ctrl_ops,
+ .id = VIVID_CID_RADIO_RX_RDS_BLOCKIO,
+ .name = "RDS Rx I/O Mode",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .qmenu = vivid_ctrl_radio_rds_mode_strings,
+ .max = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_rx_rds_rbds = {
+ .ops = &vivid_radio_rx_ctrl_ops,
+ .id = VIVID_CID_RADIO_RX_RDS_RBDS,
+ .name = "Generate RBDS Instead of RDS",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+static const char * const vivid_ctrl_radio_hw_seek_mode_strings[] = {
+ "Bounded",
+ "Wrap Around",
+ "Both",
+ NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_hw_seek_mode = {
+ .ops = &vivid_radio_rx_ctrl_ops,
+ .id = VIVID_CID_RADIO_SEEK_MODE,
+ .name = "Radio HW Seek Mode",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = 2,
+ .qmenu = vivid_ctrl_radio_hw_seek_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_hw_seek_prog_lim = {
+ .ops = &vivid_radio_rx_ctrl_ops,
+ .id = VIVID_CID_RADIO_SEEK_PROG_LIM,
+ .name = "Radio Programmable HW Seek",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .step = 1,
+};
+
+
+/* Radio Transmitter Controls */
+
+static int vivid_radio_tx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_radio_tx);
+
+ switch (ctrl->id) {
+ case VIVID_CID_RADIO_TX_RDS_BLOCKIO:
+ dev->radio_tx_rds_controls = ctrl->val;
+ dev->radio_tx_caps &= ~V4L2_CAP_READWRITE;
+ if (!dev->radio_tx_rds_controls)
+ dev->radio_tx_caps |= V4L2_CAP_READWRITE;
+ dev->radio_tx_dev.device_caps = dev->radio_tx_caps;
+ break;
+ case V4L2_CID_RDS_TX_PTY:
+ if (dev->radio_rx_rds_controls)
+ v4l2_ctrl_s_ctrl(dev->radio_rx_rds_pty, ctrl->val);
+ break;
+ case V4L2_CID_RDS_TX_PS_NAME:
+ if (dev->radio_rx_rds_controls)
+ v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_psname, ctrl->p_new.p_char);
+ break;
+ case V4L2_CID_RDS_TX_RADIO_TEXT:
+ if (dev->radio_rx_rds_controls)
+ v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_radiotext, ctrl->p_new.p_char);
+ break;
+ case V4L2_CID_RDS_TX_TRAFFIC_ANNOUNCEMENT:
+ if (dev->radio_rx_rds_controls)
+ v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ta, ctrl->val);
+ break;
+ case V4L2_CID_RDS_TX_TRAFFIC_PROGRAM:
+ if (dev->radio_rx_rds_controls)
+ v4l2_ctrl_s_ctrl(dev->radio_rx_rds_tp, ctrl->val);
+ break;
+ case V4L2_CID_RDS_TX_MUSIC_SPEECH:
+ if (dev->radio_rx_rds_controls)
+ v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ms, ctrl->val);
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_radio_tx_ctrl_ops = {
+ .s_ctrl = vivid_radio_tx_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_tx_rds_blockio = {
+ .ops = &vivid_radio_tx_ctrl_ops,
+ .id = VIVID_CID_RADIO_TX_RDS_BLOCKIO,
+ .name = "RDS Tx I/O Mode",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .qmenu = vivid_ctrl_radio_rds_mode_strings,
+ .max = 1,
+ .def = 1,
+};
+
+
+/* SDR Capture Controls */
+
+static int vivid_sdr_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_sdr_cap);
+
+ switch (ctrl->id) {
+ case VIVID_CID_SDR_CAP_FM_DEVIATION:
+ dev->sdr_fm_deviation = ctrl->val;
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_sdr_cap_ctrl_ops = {
+ .s_ctrl = vivid_sdr_cap_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_sdr_cap_fm_deviation = {
+ .ops = &vivid_sdr_cap_ctrl_ops,
+ .id = VIVID_CID_SDR_CAP_FM_DEVIATION,
+ .name = "FM Deviation",
+ .type = V4L2_CTRL_TYPE_INTEGER,
+ .min = 100,
+ .max = 200000,
+ .def = 75000,
+ .step = 1,
+};
+
+/* Metadata Capture Control */
+
+static int vivid_meta_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+ struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev,
+ ctrl_hdl_meta_cap);
+
+ switch (ctrl->id) {
+ case VIVID_CID_META_CAP_GENERATE_PTS:
+ dev->meta_pts = ctrl->val;
+ break;
+ case VIVID_CID_META_CAP_GENERATE_SCR:
+ dev->meta_scr = ctrl->val;
+ break;
+ }
+ return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_meta_cap_ctrl_ops = {
+ .s_ctrl = vivid_meta_cap_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_meta_has_pts = {
+ .ops = &vivid_meta_cap_ctrl_ops,
+ .id = VIVID_CID_META_CAP_GENERATE_PTS,
+ .name = "Generate PTS",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .def = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_meta_has_src_clk = {
+ .ops = &vivid_meta_cap_ctrl_ops,
+ .id = VIVID_CID_META_CAP_GENERATE_SCR,
+ .name = "Generate SCR",
+ .type = V4L2_CTRL_TYPE_BOOLEAN,
+ .max = 1,
+ .def = 1,
+ .step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_class = {
+ .ops = &vivid_user_gen_ctrl_ops,
+ .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
+ .id = VIVID_CID_VIVID_CLASS,
+ .name = "Vivid Controls",
+ .type = V4L2_CTRL_TYPE_CTRL_CLASS,
+};
+
+int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
+ bool show_ccs_out, bool no_error_inj,
+ bool has_sdtv, bool has_hdmi)
+{
+ struct v4l2_ctrl_handler *hdl_user_gen = &dev->ctrl_hdl_user_gen;
+ struct v4l2_ctrl_handler *hdl_user_vid = &dev->ctrl_hdl_user_vid;
+ struct v4l2_ctrl_handler *hdl_user_aud = &dev->ctrl_hdl_user_aud;
+ struct v4l2_ctrl_handler *hdl_streaming = &dev->ctrl_hdl_streaming;
+ struct v4l2_ctrl_handler *hdl_sdtv_cap = &dev->ctrl_hdl_sdtv_cap;
+ struct v4l2_ctrl_handler *hdl_loop_cap = &dev->ctrl_hdl_loop_cap;
+ struct v4l2_ctrl_handler *hdl_fb = &dev->ctrl_hdl_fb;
+ struct v4l2_ctrl_handler *hdl_vid_cap = &dev->ctrl_hdl_vid_cap;
+ struct v4l2_ctrl_handler *hdl_vid_out = &dev->ctrl_hdl_vid_out;
+ struct v4l2_ctrl_handler *hdl_vbi_cap = &dev->ctrl_hdl_vbi_cap;
+ struct v4l2_ctrl_handler *hdl_vbi_out = &dev->ctrl_hdl_vbi_out;
+ struct v4l2_ctrl_handler *hdl_radio_rx = &dev->ctrl_hdl_radio_rx;
+ struct v4l2_ctrl_handler *hdl_radio_tx = &dev->ctrl_hdl_radio_tx;
+ struct v4l2_ctrl_handler *hdl_sdr_cap = &dev->ctrl_hdl_sdr_cap;
+ struct v4l2_ctrl_handler *hdl_meta_cap = &dev->ctrl_hdl_meta_cap;
+ struct v4l2_ctrl_handler *hdl_meta_out = &dev->ctrl_hdl_meta_out;
+ struct v4l2_ctrl_handler *hdl_tch_cap = &dev->ctrl_hdl_touch_cap;
+
+ struct v4l2_ctrl_config vivid_ctrl_dv_timings = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_DV_TIMINGS,
+ .name = "DV Timings",
+ .type = V4L2_CTRL_TYPE_MENU,
+ };
+ int i;
+
+ v4l2_ctrl_handler_init(hdl_user_gen, 10);
+ v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_user_vid, 9);
+ v4l2_ctrl_new_custom(hdl_user_vid, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_user_aud, 2);
+ v4l2_ctrl_new_custom(hdl_user_aud, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_streaming, 8);
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_sdtv_cap, 2);
+ v4l2_ctrl_new_custom(hdl_sdtv_cap, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_loop_cap, 1);
+ v4l2_ctrl_new_custom(hdl_loop_cap, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_fb, 1);
+ v4l2_ctrl_new_custom(hdl_fb, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_vid_cap, 55);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_vid_out, 26);
+ if (!no_error_inj || dev->has_fb || dev->num_hdmi_outputs)
+ v4l2_ctrl_new_custom(hdl_vid_out, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_vbi_cap, 21);
+ v4l2_ctrl_new_custom(hdl_vbi_cap, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_vbi_out, 19);
+ if (!no_error_inj)
+ v4l2_ctrl_new_custom(hdl_vbi_out, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_radio_rx, 17);
+ v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_radio_tx, 17);
+ v4l2_ctrl_new_custom(hdl_radio_tx, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_sdr_cap, 19);
+ v4l2_ctrl_new_custom(hdl_sdr_cap, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_meta_cap, 2);
+ v4l2_ctrl_new_custom(hdl_meta_cap, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_meta_out, 2);
+ v4l2_ctrl_new_custom(hdl_meta_out, &vivid_ctrl_class, NULL);
+ v4l2_ctrl_handler_init(hdl_tch_cap, 2);
+ v4l2_ctrl_new_custom(hdl_tch_cap, &vivid_ctrl_class, NULL);
+
+ /* User Controls */
+ dev->volume = v4l2_ctrl_new_std(hdl_user_aud, NULL,
+ V4L2_CID_AUDIO_VOLUME, 0, 255, 1, 200);
+ dev->mute = v4l2_ctrl_new_std(hdl_user_aud, NULL,
+ V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+ if (dev->has_vid_cap) {
+ dev->brightness = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+ V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+ for (i = 0; i < MAX_INPUTS; i++)
+ dev->input_brightness[i] = 128;
+ dev->contrast = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+ V4L2_CID_CONTRAST, 0, 255, 1, 128);
+ dev->saturation = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+ V4L2_CID_SATURATION, 0, 255, 1, 128);
+ dev->hue = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+ V4L2_CID_HUE, -128, 128, 1, 0);
+ v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+ V4L2_CID_HFLIP, 0, 1, 1, 0);
+ v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+ V4L2_CID_VFLIP, 0, 1, 1, 0);
+ dev->autogain = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+ V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+ dev->gain = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+ V4L2_CID_GAIN, 0, 255, 1, 100);
+ dev->alpha = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+ V4L2_CID_ALPHA_COMPONENT, 0, 255, 1, 0);
+ }
+ dev->button = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_button, NULL);
+ dev->int32 = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int32, NULL);
+ dev->int64 = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int64, NULL);
+ dev->boolean = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_boolean, NULL);
+ dev->menu = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_menu, NULL);
+ dev->string = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_string, NULL);
+ dev->bitmask = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_bitmask, NULL);
+ dev->int_menu = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int_menu, NULL);
+ v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_area, NULL);
+ v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_u32_array, NULL);
+ v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_u16_matrix, NULL);
+ v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_u8_4d_array, NULL);
+
+ if (dev->has_vid_cap) {
+ /* Image Processing Controls */
+ struct v4l2_ctrl_config vivid_ctrl_test_pattern = {
+ .ops = &vivid_vid_cap_ctrl_ops,
+ .id = VIVID_CID_TEST_PATTERN,
+ .name = "Test Pattern",
+ .type = V4L2_CTRL_TYPE_MENU,
+ .max = TPG_PAT_NOISE,
+ .qmenu = tpg_pattern_strings,
+ };
+
+ dev->test_pattern = v4l2_ctrl_new_custom(hdl_vid_cap,
+ &vivid_ctrl_test_pattern, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_perc_fill, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_hor_movement, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_vert_movement, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_osd_mode, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_show_border, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_show_square, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_hflip, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_vflip, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_sav, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_eav, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_reduced_fps, NULL);
+ if (show_ccs_cap) {
+ dev->ctrl_has_crop_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+ &vivid_ctrl_has_crop_cap, NULL);
+ dev->ctrl_has_compose_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+ &vivid_ctrl_has_compose_cap, NULL);
+ dev->ctrl_has_scaler_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+ &vivid_ctrl_has_scaler_cap, NULL);
+ }
+
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_tstamp_src, NULL);
+ dev->colorspace = v4l2_ctrl_new_custom(hdl_vid_cap,
+ &vivid_ctrl_colorspace, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_xfer_func, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_ycbcr_enc, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_hsv_enc, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_quantization, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_alpha_mode, NULL);
+ }
+
+ if (dev->has_vid_out && show_ccs_out) {
+ dev->ctrl_has_crop_out = v4l2_ctrl_new_custom(hdl_vid_out,
+ &vivid_ctrl_has_crop_out, NULL);
+ dev->ctrl_has_compose_out = v4l2_ctrl_new_custom(hdl_vid_out,
+ &vivid_ctrl_has_compose_out, NULL);
+ dev->ctrl_has_scaler_out = v4l2_ctrl_new_custom(hdl_vid_out,
+ &vivid_ctrl_has_scaler_out, NULL);
+ }
+
+ /*
+ * Testing this driver with v4l2-compliance will trigger the error
+ * injection controls, and after that nothing will work as expected.
+ * So we have a module option to drop these error injecting controls
+ * allowing us to run v4l2_compliance again.
+ */
+ if (!no_error_inj) {
+ v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_disconnect, NULL);
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_dqbuf_error, NULL);
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_perc_dropped, NULL);
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_queue_setup_error, NULL);
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_buf_prepare_error, NULL);
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_start_streaming_error, NULL);
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_queue_error, NULL);
+#ifdef CONFIG_MEDIA_CONTROLLER
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_req_validate_error, NULL);
+#endif
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_seq_wrap, NULL);
+ v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_time_wrap, NULL);
+ }
+
+ if (has_sdtv && (dev->has_vid_cap || dev->has_vbi_cap)) {
+ if (dev->has_vid_cap)
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_std_aspect_ratio, NULL);
+ dev->ctrl_std_signal_mode = v4l2_ctrl_new_custom(hdl_sdtv_cap,
+ &vivid_ctrl_std_signal_mode, NULL);
+ dev->ctrl_standard = v4l2_ctrl_new_custom(hdl_sdtv_cap,
+ &vivid_ctrl_standard, NULL);
+ if (dev->ctrl_std_signal_mode)
+ v4l2_ctrl_cluster(2, &dev->ctrl_std_signal_mode);
+ if (dev->has_raw_vbi_cap)
+ v4l2_ctrl_new_custom(hdl_vbi_cap, &vivid_ctrl_vbi_cap_interlaced, NULL);
+ }
+
+ if (dev->num_hdmi_inputs) {
+ s64 hdmi_input_mask = GENMASK(dev->num_hdmi_inputs - 1, 0);
+
+ dev->ctrl_dv_timings_signal_mode = v4l2_ctrl_new_custom(hdl_vid_cap,
+ &vivid_ctrl_dv_timings_signal_mode, NULL);
+
+ vivid_ctrl_dv_timings.max = dev->query_dv_timings_size - 1;
+ vivid_ctrl_dv_timings.qmenu =
+ (const char * const *)dev->query_dv_timings_qmenu;
+ dev->ctrl_dv_timings = v4l2_ctrl_new_custom(hdl_vid_cap,
+ &vivid_ctrl_dv_timings, NULL);
+ if (dev->ctrl_dv_timings_signal_mode)
+ v4l2_ctrl_cluster(2, &dev->ctrl_dv_timings_signal_mode);
+
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_dv_timings_aspect_ratio, NULL);
+ v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_max_edid_blocks, NULL);
+ dev->real_rgb_range_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+ &vivid_ctrl_limited_rgb_range, NULL);
+ dev->rgb_range_cap = v4l2_ctrl_new_std_menu(hdl_vid_cap,
+ &vivid_vid_cap_ctrl_ops,
+ V4L2_CID_DV_RX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+ 0, V4L2_DV_RGB_RANGE_AUTO);
+ dev->ctrl_rx_power_present = v4l2_ctrl_new_std(hdl_vid_cap,
+ NULL, V4L2_CID_DV_RX_POWER_PRESENT, 0, hdmi_input_mask,
+ 0, hdmi_input_mask);
+
+ }
+ if (dev->num_hdmi_outputs) {
+ s64 hdmi_output_mask = GENMASK(dev->num_hdmi_outputs - 1, 0);
+
+ /*
+ * We aren't doing anything with this at the moment, but
+ * HDMI outputs typically have this controls.
+ */
+ dev->ctrl_tx_rgb_range = v4l2_ctrl_new_std_menu(hdl_vid_out, NULL,
+ V4L2_CID_DV_TX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+ 0, V4L2_DV_RGB_RANGE_AUTO);
+ dev->ctrl_tx_mode = v4l2_ctrl_new_std_menu(hdl_vid_out, NULL,
+ V4L2_CID_DV_TX_MODE, V4L2_DV_TX_MODE_HDMI,
+ 0, V4L2_DV_TX_MODE_HDMI);
+ dev->ctrl_display_present = v4l2_ctrl_new_custom(hdl_vid_out,
+ &vivid_ctrl_display_present, NULL);
+ dev->ctrl_tx_hotplug = v4l2_ctrl_new_std(hdl_vid_out,
+ NULL, V4L2_CID_DV_TX_HOTPLUG, 0, hdmi_output_mask,
+ 0, hdmi_output_mask);
+ dev->ctrl_tx_rxsense = v4l2_ctrl_new_std(hdl_vid_out,
+ NULL, V4L2_CID_DV_TX_RXSENSE, 0, hdmi_output_mask,
+ 0, hdmi_output_mask);
+ dev->ctrl_tx_edid_present = v4l2_ctrl_new_std(hdl_vid_out,
+ NULL, V4L2_CID_DV_TX_EDID_PRESENT, 0, hdmi_output_mask,
+ 0, hdmi_output_mask);
+ }
+ if ((dev->has_vid_cap && dev->has_vid_out) ||
+ (dev->has_vbi_cap && dev->has_vbi_out))
+ v4l2_ctrl_new_custom(hdl_loop_cap, &vivid_ctrl_loop_video, NULL);
+
+ if (dev->has_fb)
+ v4l2_ctrl_new_custom(hdl_fb, &vivid_ctrl_clear_fb, NULL);
+
+ if (dev->has_radio_rx) {
+ v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_hw_seek_mode, NULL);
+ v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_hw_seek_prog_lim, NULL);
+ v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_rx_rds_blockio, NULL);
+ v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_rx_rds_rbds, NULL);
+ v4l2_ctrl_new_std(hdl_radio_rx, &vivid_radio_rx_ctrl_ops,
+ V4L2_CID_RDS_RECEPTION, 0, 1, 1, 1);
+ dev->radio_rx_rds_pty = v4l2_ctrl_new_std(hdl_radio_rx,
+ &vivid_radio_rx_ctrl_ops,
+ V4L2_CID_RDS_RX_PTY, 0, 31, 1, 0);
+ dev->radio_rx_rds_psname = v4l2_ctrl_new_std(hdl_radio_rx,
+ &vivid_radio_rx_ctrl_ops,
+ V4L2_CID_RDS_RX_PS_NAME, 0, 8, 8, 0);
+ dev->radio_rx_rds_radiotext = v4l2_ctrl_new_std(hdl_radio_rx,
+ &vivid_radio_rx_ctrl_ops,
+ V4L2_CID_RDS_RX_RADIO_TEXT, 0, 64, 64, 0);
+ dev->radio_rx_rds_ta = v4l2_ctrl_new_std(hdl_radio_rx,
+ &vivid_radio_rx_ctrl_ops,
+ V4L2_CID_RDS_RX_TRAFFIC_ANNOUNCEMENT, 0, 1, 1, 0);
+ dev->radio_rx_rds_tp = v4l2_ctrl_new_std(hdl_radio_rx,
+ &vivid_radio_rx_ctrl_ops,
+ V4L2_CID_RDS_RX_TRAFFIC_PROGRAM, 0, 1, 1, 0);
+ dev->radio_rx_rds_ms = v4l2_ctrl_new_std(hdl_radio_rx,
+ &vivid_radio_rx_ctrl_ops,
+ V4L2_CID_RDS_RX_MUSIC_SPEECH, 0, 1, 1, 1);
+ }
+ if (dev->has_radio_tx) {
+ v4l2_ctrl_new_custom(hdl_radio_tx,
+ &vivid_ctrl_radio_tx_rds_blockio, NULL);
+ dev->radio_tx_rds_pi = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_PI, 0, 0xffff, 1, 0x8088);
+ dev->radio_tx_rds_pty = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_PTY, 0, 31, 1, 3);
+ dev->radio_tx_rds_psname = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_PS_NAME, 0, 8, 8, 0);
+ if (dev->radio_tx_rds_psname)
+ v4l2_ctrl_s_ctrl_string(dev->radio_tx_rds_psname, "VIVID-TX");
+ dev->radio_tx_rds_radiotext = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_RADIO_TEXT, 0, 64 * 2, 64, 0);
+ if (dev->radio_tx_rds_radiotext)
+ v4l2_ctrl_s_ctrl_string(dev->radio_tx_rds_radiotext,
+ "This is a VIVID default Radio Text template text, change at will");
+ dev->radio_tx_rds_mono_stereo = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_MONO_STEREO, 0, 1, 1, 1);
+ dev->radio_tx_rds_art_head = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_ARTIFICIAL_HEAD, 0, 1, 1, 0);
+ dev->radio_tx_rds_compressed = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_COMPRESSED, 0, 1, 1, 0);
+ dev->radio_tx_rds_dyn_pty = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_DYNAMIC_PTY, 0, 1, 1, 0);
+ dev->radio_tx_rds_ta = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_TRAFFIC_ANNOUNCEMENT, 0, 1, 1, 0);
+ dev->radio_tx_rds_tp = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_TRAFFIC_PROGRAM, 0, 1, 1, 1);
+ dev->radio_tx_rds_ms = v4l2_ctrl_new_std(hdl_radio_tx,
+ &vivid_radio_tx_ctrl_ops,
+ V4L2_CID_RDS_TX_MUSIC_SPEECH, 0, 1, 1, 1);
+ }
+ if (dev->has_sdr_cap) {
+ v4l2_ctrl_new_custom(hdl_sdr_cap,
+ &vivid_ctrl_sdr_cap_fm_deviation, NULL);
+ }
+ if (dev->has_meta_cap) {
+ v4l2_ctrl_new_custom(hdl_meta_cap,
+ &vivid_ctrl_meta_has_pts, NULL);
+ v4l2_ctrl_new_custom(hdl_meta_cap,
+ &vivid_ctrl_meta_has_src_clk, NULL);
+ }
+
+ if (hdl_user_gen->error)
+ return hdl_user_gen->error;
+ if (hdl_user_vid->error)
+ return hdl_user_vid->error;
+ if (hdl_user_aud->error)
+ return hdl_user_aud->error;
+ if (hdl_streaming->error)
+ return hdl_streaming->error;
+ if (hdl_sdr_cap->error)
+ return hdl_sdr_cap->error;
+ if (hdl_loop_cap->error)
+ return hdl_loop_cap->error;
+
+ if (dev->autogain)
+ v4l2_ctrl_auto_cluster(2, &dev->autogain, 0, true);
+
+ if (dev->has_vid_cap) {
+ v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_vid, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_aud, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vid_cap, hdl_streaming, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vid_cap, hdl_sdtv_cap, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vid_cap, hdl_loop_cap, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vid_cap, hdl_fb, NULL, false);
+ if (hdl_vid_cap->error)
+ return hdl_vid_cap->error;
+ dev->vid_cap_dev.ctrl_handler = hdl_vid_cap;
+ }
+ if (dev->has_vid_out) {
+ v4l2_ctrl_add_handler(hdl_vid_out, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vid_out, hdl_user_aud, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vid_out, hdl_streaming, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vid_out, hdl_fb, NULL, false);
+ if (hdl_vid_out->error)
+ return hdl_vid_out->error;
+ dev->vid_out_dev.ctrl_handler = hdl_vid_out;
+ }
+ if (dev->has_vbi_cap) {
+ v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_streaming, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_sdtv_cap, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_loop_cap, NULL, false);
+ if (hdl_vbi_cap->error)
+ return hdl_vbi_cap->error;
+ dev->vbi_cap_dev.ctrl_handler = hdl_vbi_cap;
+ }
+ if (dev->has_vbi_out) {
+ v4l2_ctrl_add_handler(hdl_vbi_out, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_vbi_out, hdl_streaming, NULL, false);
+ if (hdl_vbi_out->error)
+ return hdl_vbi_out->error;
+ dev->vbi_out_dev.ctrl_handler = hdl_vbi_out;
+ }
+ if (dev->has_radio_rx) {
+ v4l2_ctrl_add_handler(hdl_radio_rx, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_radio_rx, hdl_user_aud, NULL, false);
+ if (hdl_radio_rx->error)
+ return hdl_radio_rx->error;
+ dev->radio_rx_dev.ctrl_handler = hdl_radio_rx;
+ }
+ if (dev->has_radio_tx) {
+ v4l2_ctrl_add_handler(hdl_radio_tx, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_radio_tx, hdl_user_aud, NULL, false);
+ if (hdl_radio_tx->error)
+ return hdl_radio_tx->error;
+ dev->radio_tx_dev.ctrl_handler = hdl_radio_tx;
+ }
+ if (dev->has_sdr_cap) {
+ v4l2_ctrl_add_handler(hdl_sdr_cap, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_sdr_cap, hdl_streaming, NULL, false);
+ if (hdl_sdr_cap->error)
+ return hdl_sdr_cap->error;
+ dev->sdr_cap_dev.ctrl_handler = hdl_sdr_cap;
+ }
+ if (dev->has_meta_cap) {
+ v4l2_ctrl_add_handler(hdl_meta_cap, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_meta_cap, hdl_streaming, NULL, false);
+ if (hdl_meta_cap->error)
+ return hdl_meta_cap->error;
+ dev->meta_cap_dev.ctrl_handler = hdl_meta_cap;
+ }
+ if (dev->has_meta_out) {
+ v4l2_ctrl_add_handler(hdl_meta_out, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_meta_out, hdl_streaming, NULL, false);
+ if (hdl_meta_out->error)
+ return hdl_meta_out->error;
+ dev->meta_out_dev.ctrl_handler = hdl_meta_out;
+ }
+ if (dev->has_touch_cap) {
+ v4l2_ctrl_add_handler(hdl_tch_cap, hdl_user_gen, NULL, false);
+ v4l2_ctrl_add_handler(hdl_tch_cap, hdl_streaming, NULL, false);
+ if (hdl_tch_cap->error)
+ return hdl_tch_cap->error;
+ dev->touch_cap_dev.ctrl_handler = hdl_tch_cap;
+ }
+ return 0;
+}
+
+void vivid_free_controls(struct vivid_dev *dev)
+{
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_vid_cap);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_vid_out);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_vbi_cap);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_vbi_out);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_radio_rx);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_radio_tx);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_sdr_cap);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_gen);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_vid);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_aud);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_streaming);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_sdtv_cap);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_loop_cap);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_fb);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_meta_cap);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_meta_out);
+ v4l2_ctrl_handler_free(&dev->ctrl_hdl_touch_cap);
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-ctrls.h b/drivers/media/test-drivers/vivid/vivid-ctrls.h
new file mode 100644
index 0000000..6fad5f5
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-ctrls.h
@@ -0,0 +1,22 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-ctrls.h - control support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_CTRLS_H_
+#define _VIVID_CTRLS_H_
+
+enum vivid_hw_seek_modes {
+ VIVID_HW_SEEK_BOUNDED,
+ VIVID_HW_SEEK_WRAP,
+ VIVID_HW_SEEK_BOTH,
+};
+
+int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
+ bool show_ccs_out, bool no_error_inj,
+ bool has_sdtv, bool has_hdmi);
+void vivid_free_controls(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-kthread-cap.c b/drivers/media/test-drivers/vivid/vivid-kthread-cap.c
new file mode 100644
index 0000000..01a9d67
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-kthread-cap.c
@@ -0,0 +1,1007 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-kthread-cap.h - video/vbi capture thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/random.h>
+#include <linux/v4l2-dv-timings.h>
+#include <asm/div64.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-rect.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-rx.h"
+#include "vivid-radio-tx.h"
+#include "vivid-sdr-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-out.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+#include "vivid-kthread-cap.h"
+#include "vivid-meta-cap.h"
+
+static inline v4l2_std_id vivid_get_std_cap(const struct vivid_dev *dev)
+{
+ if (vivid_is_sdtv_cap(dev))
+ return dev->std_cap[dev->input];
+ return 0;
+}
+
+static void copy_pix(struct vivid_dev *dev, int win_y, int win_x,
+ u16 *cap, const u16 *osd)
+{
+ u16 out;
+ int left = dev->overlay_out_left;
+ int top = dev->overlay_out_top;
+ int fb_x = win_x + left;
+ int fb_y = win_y + top;
+ int i;
+
+ out = *cap;
+ *cap = *osd;
+ if (dev->bitmap_out) {
+ const u8 *p = dev->bitmap_out;
+ unsigned stride = (dev->compose_out.width + 7) / 8;
+
+ win_x -= dev->compose_out.left;
+ win_y -= dev->compose_out.top;
+ if (!(p[stride * win_y + win_x / 8] & (1 << (win_x & 7))))
+ return;
+ }
+
+ for (i = 0; i < dev->clipcount_out; i++) {
+ struct v4l2_rect *r = &dev->clips_out[i].c;
+
+ if (fb_y >= r->top && fb_y < r->top + r->height &&
+ fb_x >= r->left && fb_x < r->left + r->width)
+ return;
+ }
+ if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_CHROMAKEY) &&
+ *osd != dev->chromakey_out)
+ return;
+ if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_SRC_CHROMAKEY) &&
+ out == dev->chromakey_out)
+ return;
+ if (dev->fmt_cap->alpha_mask) {
+ if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_GLOBAL_ALPHA) &&
+ dev->global_alpha_out)
+ return;
+ if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_LOCAL_ALPHA) &&
+ *cap & dev->fmt_cap->alpha_mask)
+ return;
+ if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_LOCAL_INV_ALPHA) &&
+ !(*cap & dev->fmt_cap->alpha_mask))
+ return;
+ }
+ *cap = out;
+}
+
+static void blend_line(struct vivid_dev *dev, unsigned y_offset, unsigned x_offset,
+ u8 *vcapbuf, const u8 *vosdbuf,
+ unsigned width, unsigned pixsize)
+{
+ unsigned x;
+
+ for (x = 0; x < width; x++, vcapbuf += pixsize, vosdbuf += pixsize) {
+ copy_pix(dev, y_offset, x_offset + x,
+ (u16 *)vcapbuf, (const u16 *)vosdbuf);
+ }
+}
+
+static void scale_line(const u8 *src, u8 *dst, unsigned srcw, unsigned dstw, unsigned twopixsize)
+{
+ /* Coarse scaling with Bresenham */
+ unsigned int_part;
+ unsigned fract_part;
+ unsigned src_x = 0;
+ unsigned error = 0;
+ unsigned x;
+
+ /*
+ * We always combine two pixels to prevent color bleed in the packed
+ * yuv case.
+ */
+ srcw /= 2;
+ dstw /= 2;
+ int_part = srcw / dstw;
+ fract_part = srcw % dstw;
+ for (x = 0; x < dstw; x++, dst += twopixsize) {
+ memcpy(dst, src + src_x * twopixsize, twopixsize);
+ src_x += int_part;
+ error += fract_part;
+ if (error >= dstw) {
+ error -= dstw;
+ src_x++;
+ }
+ }
+}
+
+/*
+ * Precalculate the rectangles needed to perform video looping:
+ *
+ * The nominal pipeline is that the video output buffer is cropped by
+ * crop_out, scaled to compose_out, overlaid with the output overlay,
+ * cropped on the capture side by crop_cap and scaled again to the video
+ * capture buffer using compose_cap.
+ *
+ * To keep things efficient we calculate the intersection of compose_out
+ * and crop_cap (since that's the only part of the video that will
+ * actually end up in the capture buffer), determine which part of the
+ * video output buffer that is and which part of the video capture buffer
+ * so we can scale the video straight from the output buffer to the capture
+ * buffer without any intermediate steps.
+ *
+ * If we need to deal with an output overlay, then there is no choice and
+ * that intermediate step still has to be taken. For the output overlay
+ * support we calculate the intersection of the framebuffer and the overlay
+ * window (which may be partially or wholly outside of the framebuffer
+ * itself) and the intersection of that with loop_vid_copy (i.e. the part of
+ * the actual looped video that will be overlaid). The result is calculated
+ * both in framebuffer coordinates (loop_fb_copy) and compose_out coordinates
+ * (loop_vid_overlay). Finally calculate the part of the capture buffer that
+ * will receive that overlaid video.
+ */
+static void vivid_precalc_copy_rects(struct vivid_dev *dev)
+{
+ /* Framebuffer rectangle */
+ struct v4l2_rect r_fb = {
+ 0, 0, dev->display_width, dev->display_height
+ };
+ /* Overlay window rectangle in framebuffer coordinates */
+ struct v4l2_rect r_overlay = {
+ dev->overlay_out_left, dev->overlay_out_top,
+ dev->compose_out.width, dev->compose_out.height
+ };
+
+ v4l2_rect_intersect(&dev->loop_vid_copy, &dev->crop_cap, &dev->compose_out);
+
+ dev->loop_vid_out = dev->loop_vid_copy;
+ v4l2_rect_scale(&dev->loop_vid_out, &dev->compose_out, &dev->crop_out);
+ dev->loop_vid_out.left += dev->crop_out.left;
+ dev->loop_vid_out.top += dev->crop_out.top;
+
+ dev->loop_vid_cap = dev->loop_vid_copy;
+ v4l2_rect_scale(&dev->loop_vid_cap, &dev->crop_cap, &dev->compose_cap);
+
+ dprintk(dev, 1,
+ "loop_vid_copy: %dx%d@%dx%d loop_vid_out: %dx%d@%dx%d loop_vid_cap: %dx%d@%dx%d\n",
+ dev->loop_vid_copy.width, dev->loop_vid_copy.height,
+ dev->loop_vid_copy.left, dev->loop_vid_copy.top,
+ dev->loop_vid_out.width, dev->loop_vid_out.height,
+ dev->loop_vid_out.left, dev->loop_vid_out.top,
+ dev->loop_vid_cap.width, dev->loop_vid_cap.height,
+ dev->loop_vid_cap.left, dev->loop_vid_cap.top);
+
+ v4l2_rect_intersect(&r_overlay, &r_fb, &r_overlay);
+
+ /* shift r_overlay to the same origin as compose_out */
+ r_overlay.left += dev->compose_out.left - dev->overlay_out_left;
+ r_overlay.top += dev->compose_out.top - dev->overlay_out_top;
+
+ v4l2_rect_intersect(&dev->loop_vid_overlay, &r_overlay, &dev->loop_vid_copy);
+ dev->loop_fb_copy = dev->loop_vid_overlay;
+
+ /* shift dev->loop_fb_copy back again to the fb origin */
+ dev->loop_fb_copy.left -= dev->compose_out.left - dev->overlay_out_left;
+ dev->loop_fb_copy.top -= dev->compose_out.top - dev->overlay_out_top;
+
+ dev->loop_vid_overlay_cap = dev->loop_vid_overlay;
+ v4l2_rect_scale(&dev->loop_vid_overlay_cap, &dev->crop_cap, &dev->compose_cap);
+
+ dprintk(dev, 1,
+ "loop_fb_copy: %dx%d@%dx%d loop_vid_overlay: %dx%d@%dx%d loop_vid_overlay_cap: %dx%d@%dx%d\n",
+ dev->loop_fb_copy.width, dev->loop_fb_copy.height,
+ dev->loop_fb_copy.left, dev->loop_fb_copy.top,
+ dev->loop_vid_overlay.width, dev->loop_vid_overlay.height,
+ dev->loop_vid_overlay.left, dev->loop_vid_overlay.top,
+ dev->loop_vid_overlay_cap.width, dev->loop_vid_overlay_cap.height,
+ dev->loop_vid_overlay_cap.left, dev->loop_vid_overlay_cap.top);
+}
+
+static void *plane_vaddr(struct tpg_data *tpg, struct vivid_buffer *buf,
+ unsigned p, unsigned bpl[TPG_MAX_PLANES], unsigned h)
+{
+ unsigned i;
+ void *vbuf;
+
+ if (p == 0 || tpg_g_buffers(tpg) > 1)
+ return vb2_plane_vaddr(&buf->vb.vb2_buf, p);
+ vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+ for (i = 0; i < p; i++)
+ vbuf += bpl[i] * h / tpg->vdownsampling[i];
+ return vbuf;
+}
+
+static noinline_for_stack int vivid_copy_buffer(struct vivid_dev *dev, unsigned p,
+ u8 *vcapbuf, struct vivid_buffer *vid_cap_buf)
+{
+ bool blank = dev->must_blank[vid_cap_buf->vb.vb2_buf.index];
+ struct tpg_data *tpg = &dev->tpg;
+ struct vivid_buffer *vid_out_buf = NULL;
+ unsigned vdiv = dev->fmt_out->vdownsampling[p];
+ unsigned twopixsize = tpg_g_twopixelsize(tpg, p);
+ unsigned img_width = tpg_hdiv(tpg, p, dev->compose_cap.width);
+ unsigned img_height = dev->compose_cap.height;
+ unsigned stride_cap = tpg->bytesperline[p];
+ unsigned stride_out = dev->bytesperline_out[p];
+ unsigned stride_osd = dev->display_byte_stride;
+ unsigned hmax = (img_height * tpg->perc_fill) / 100;
+ u8 *voutbuf;
+ u8 *vosdbuf = NULL;
+ unsigned y;
+ bool blend = dev->bitmap_out || dev->clipcount_out || dev->fbuf_out_flags;
+ /* Coarse scaling with Bresenham */
+ unsigned vid_out_int_part;
+ unsigned vid_out_fract_part;
+ unsigned vid_out_y = 0;
+ unsigned vid_out_error = 0;
+ unsigned vid_overlay_int_part = 0;
+ unsigned vid_overlay_fract_part = 0;
+ unsigned vid_overlay_y = 0;
+ unsigned vid_overlay_error = 0;
+ unsigned vid_cap_left = tpg_hdiv(tpg, p, dev->loop_vid_cap.left);
+ unsigned vid_cap_right;
+ bool quick;
+
+ vid_out_int_part = dev->loop_vid_out.height / dev->loop_vid_cap.height;
+ vid_out_fract_part = dev->loop_vid_out.height % dev->loop_vid_cap.height;
+
+ if (!list_empty(&dev->vid_out_active))
+ vid_out_buf = list_entry(dev->vid_out_active.next,
+ struct vivid_buffer, list);
+ if (vid_out_buf == NULL)
+ return -ENODATA;
+
+ vid_cap_buf->vb.field = vid_out_buf->vb.field;
+
+ voutbuf = plane_vaddr(tpg, vid_out_buf, p,
+ dev->bytesperline_out, dev->fmt_out_rect.height);
+ if (p < dev->fmt_out->buffers)
+ voutbuf += vid_out_buf->vb.vb2_buf.planes[p].data_offset;
+ voutbuf += tpg_hdiv(tpg, p, dev->loop_vid_out.left) +
+ (dev->loop_vid_out.top / vdiv) * stride_out;
+ vcapbuf += tpg_hdiv(tpg, p, dev->compose_cap.left) +
+ (dev->compose_cap.top / vdiv) * stride_cap;
+
+ if (dev->loop_vid_copy.width == 0 || dev->loop_vid_copy.height == 0) {
+ /*
+ * If there is nothing to copy, then just fill the capture window
+ * with black.
+ */
+ for (y = 0; y < hmax / vdiv; y++, vcapbuf += stride_cap)
+ memcpy(vcapbuf, tpg->black_line[p], img_width);
+ return 0;
+ }
+
+ if (dev->overlay_out_enabled &&
+ dev->loop_vid_overlay.width && dev->loop_vid_overlay.height) {
+ vosdbuf = dev->video_vbase;
+ vosdbuf += (dev->loop_fb_copy.left * twopixsize) / 2 +
+ dev->loop_fb_copy.top * stride_osd;
+ vid_overlay_int_part = dev->loop_vid_overlay.height /
+ dev->loop_vid_overlay_cap.height;
+ vid_overlay_fract_part = dev->loop_vid_overlay.height %
+ dev->loop_vid_overlay_cap.height;
+ }
+
+ vid_cap_right = tpg_hdiv(tpg, p, dev->loop_vid_cap.left + dev->loop_vid_cap.width);
+ /* quick is true if no video scaling is needed */
+ quick = dev->loop_vid_out.width == dev->loop_vid_cap.width;
+
+ dev->cur_scaled_line = dev->loop_vid_out.height;
+ for (y = 0; y < hmax; y += vdiv, vcapbuf += stride_cap) {
+ /* osdline is true if this line requires overlay blending */
+ bool osdline = vosdbuf && y >= dev->loop_vid_overlay_cap.top &&
+ y < dev->loop_vid_overlay_cap.top + dev->loop_vid_overlay_cap.height;
+
+ /*
+ * If this line of the capture buffer doesn't get any video, then
+ * just fill with black.
+ */
+ if (y < dev->loop_vid_cap.top ||
+ y >= dev->loop_vid_cap.top + dev->loop_vid_cap.height) {
+ memcpy(vcapbuf, tpg->black_line[p], img_width);
+ continue;
+ }
+
+ /* fill the left border with black */
+ if (dev->loop_vid_cap.left)
+ memcpy(vcapbuf, tpg->black_line[p], vid_cap_left);
+
+ /* fill the right border with black */
+ if (vid_cap_right < img_width)
+ memcpy(vcapbuf + vid_cap_right, tpg->black_line[p],
+ img_width - vid_cap_right);
+
+ if (quick && !osdline) {
+ memcpy(vcapbuf + vid_cap_left,
+ voutbuf + vid_out_y * stride_out,
+ tpg_hdiv(tpg, p, dev->loop_vid_cap.width));
+ goto update_vid_out_y;
+ }
+ if (dev->cur_scaled_line == vid_out_y) {
+ memcpy(vcapbuf + vid_cap_left, dev->scaled_line,
+ tpg_hdiv(tpg, p, dev->loop_vid_cap.width));
+ goto update_vid_out_y;
+ }
+ if (!osdline) {
+ scale_line(voutbuf + vid_out_y * stride_out, dev->scaled_line,
+ tpg_hdiv(tpg, p, dev->loop_vid_out.width),
+ tpg_hdiv(tpg, p, dev->loop_vid_cap.width),
+ tpg_g_twopixelsize(tpg, p));
+ } else {
+ /*
+ * Offset in bytes within loop_vid_copy to the start of the
+ * loop_vid_overlay rectangle.
+ */
+ unsigned offset =
+ ((dev->loop_vid_overlay.left - dev->loop_vid_copy.left) *
+ twopixsize) / 2;
+ u8 *osd = vosdbuf + vid_overlay_y * stride_osd;
+
+ scale_line(voutbuf + vid_out_y * stride_out, dev->blended_line,
+ dev->loop_vid_out.width, dev->loop_vid_copy.width,
+ tpg_g_twopixelsize(tpg, p));
+ if (blend)
+ blend_line(dev, vid_overlay_y + dev->loop_vid_overlay.top,
+ dev->loop_vid_overlay.left,
+ dev->blended_line + offset, osd,
+ dev->loop_vid_overlay.width, twopixsize / 2);
+ else
+ memcpy(dev->blended_line + offset,
+ osd, (dev->loop_vid_overlay.width * twopixsize) / 2);
+ scale_line(dev->blended_line, dev->scaled_line,
+ dev->loop_vid_copy.width, dev->loop_vid_cap.width,
+ tpg_g_twopixelsize(tpg, p));
+ }
+ dev->cur_scaled_line = vid_out_y;
+ memcpy(vcapbuf + vid_cap_left, dev->scaled_line,
+ tpg_hdiv(tpg, p, dev->loop_vid_cap.width));
+
+update_vid_out_y:
+ if (osdline) {
+ vid_overlay_y += vid_overlay_int_part;
+ vid_overlay_error += vid_overlay_fract_part;
+ if (vid_overlay_error >= dev->loop_vid_overlay_cap.height) {
+ vid_overlay_error -= dev->loop_vid_overlay_cap.height;
+ vid_overlay_y++;
+ }
+ }
+ vid_out_y += vid_out_int_part;
+ vid_out_error += vid_out_fract_part;
+ if (vid_out_error >= dev->loop_vid_cap.height / vdiv) {
+ vid_out_error -= dev->loop_vid_cap.height / vdiv;
+ vid_out_y++;
+ }
+ }
+
+ if (!blank)
+ return 0;
+ for (; y < img_height; y += vdiv, vcapbuf += stride_cap)
+ memcpy(vcapbuf, tpg->contrast_line[p], img_width);
+ return 0;
+}
+
+static void vivid_fillbuff(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+ struct tpg_data *tpg = &dev->tpg;
+ unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_cap) ? 2 : 1;
+ unsigned line_height = 16 / factor;
+ bool is_tv = vivid_is_sdtv_cap(dev);
+ bool is_60hz = is_tv && (dev->std_cap[dev->input] & V4L2_STD_525_60);
+ unsigned p;
+ int line = 1;
+ u8 *basep[TPG_MAX_PLANES][2];
+ unsigned ms;
+ char str[100];
+ s32 gain;
+ bool is_loop = false;
+
+ if (dev->loop_video && dev->can_loop_video &&
+ ((vivid_is_svid_cap(dev) &&
+ !VIVID_INVALID_SIGNAL(dev->std_signal_mode[dev->input])) ||
+ (vivid_is_hdmi_cap(dev) &&
+ !VIVID_INVALID_SIGNAL(dev->dv_timings_signal_mode[dev->input]))))
+ is_loop = true;
+
+ buf->vb.sequence = dev->vid_cap_seq_count;
+ if (dev->field_cap == V4L2_FIELD_ALTERNATE) {
+ /*
+ * 60 Hz standards start with the bottom field, 50 Hz standards
+ * with the top field. So if the 0-based seq_count is even,
+ * then the field is TOP for 50 Hz and BOTTOM for 60 Hz
+ * standards.
+ */
+ buf->vb.field = ((dev->vid_cap_seq_count & 1) ^ is_60hz) ?
+ V4L2_FIELD_BOTTOM : V4L2_FIELD_TOP;
+ /*
+ * The sequence counter counts frames, not fields. So divide
+ * by two.
+ */
+ buf->vb.sequence /= 2;
+ } else {
+ buf->vb.field = dev->field_cap;
+ }
+ tpg_s_field(tpg, buf->vb.field,
+ dev->field_cap == V4L2_FIELD_ALTERNATE);
+ tpg_s_perc_fill_blank(tpg, dev->must_blank[buf->vb.vb2_buf.index]);
+
+ vivid_precalc_copy_rects(dev);
+
+ for (p = 0; p < tpg_g_planes(tpg); p++) {
+ void *vbuf = plane_vaddr(tpg, buf, p,
+ tpg->bytesperline, tpg->buf_height);
+
+ /*
+ * The first plane of a multiplanar format has a non-zero
+ * data_offset. This helps testing whether the application
+ * correctly supports non-zero data offsets.
+ */
+ if (p < tpg_g_buffers(tpg) && dev->fmt_cap->data_offset[p]) {
+ memset(vbuf, dev->fmt_cap->data_offset[p] & 0xff,
+ dev->fmt_cap->data_offset[p]);
+ vbuf += dev->fmt_cap->data_offset[p];
+ }
+ tpg_calc_text_basep(tpg, basep, p, vbuf);
+ if (!is_loop || vivid_copy_buffer(dev, p, vbuf, buf))
+ tpg_fill_plane_buffer(tpg, vivid_get_std_cap(dev),
+ p, vbuf);
+ }
+ dev->must_blank[buf->vb.vb2_buf.index] = false;
+
+ /* Updates stream time, only update at the start of a new frame. */
+ if (dev->field_cap != V4L2_FIELD_ALTERNATE ||
+ (dev->vid_cap_seq_count & 1) == 0)
+ dev->ms_vid_cap =
+ jiffies_to_msecs(jiffies - dev->jiffies_vid_cap);
+
+ ms = dev->ms_vid_cap;
+ if (dev->osd_mode <= 1) {
+ snprintf(str, sizeof(str), " %02d:%02d:%02d:%03d %u%s",
+ (ms / (60 * 60 * 1000)) % 24,
+ (ms / (60 * 1000)) % 60,
+ (ms / 1000) % 60,
+ ms % 1000,
+ buf->vb.sequence,
+ (dev->field_cap == V4L2_FIELD_ALTERNATE) ?
+ (buf->vb.field == V4L2_FIELD_TOP ?
+ " top" : " bottom") : "");
+ tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+ }
+ if (dev->osd_mode == 0) {
+ snprintf(str, sizeof(str), " %dx%d, input %d ",
+ dev->src_rect.width, dev->src_rect.height, dev->input);
+ tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+
+ gain = v4l2_ctrl_g_ctrl(dev->gain);
+ mutex_lock(dev->ctrl_hdl_user_vid.lock);
+ snprintf(str, sizeof(str),
+ " brightness %3d, contrast %3d, saturation %3d, hue %d ",
+ dev->brightness->cur.val,
+ dev->contrast->cur.val,
+ dev->saturation->cur.val,
+ dev->hue->cur.val);
+ tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+ snprintf(str, sizeof(str),
+ " autogain %d, gain %3d, alpha 0x%02x ",
+ dev->autogain->cur.val, gain, dev->alpha->cur.val);
+ mutex_unlock(dev->ctrl_hdl_user_vid.lock);
+ tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+ mutex_lock(dev->ctrl_hdl_user_aud.lock);
+ snprintf(str, sizeof(str),
+ " volume %3d, mute %d ",
+ dev->volume->cur.val, dev->mute->cur.val);
+ mutex_unlock(dev->ctrl_hdl_user_aud.lock);
+ tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+ mutex_lock(dev->ctrl_hdl_user_gen.lock);
+ snprintf(str, sizeof(str), " int32 %d, int64 %lld, bitmask %08x ",
+ dev->int32->cur.val,
+ *dev->int64->p_cur.p_s64,
+ dev->bitmask->cur.val);
+ tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+ snprintf(str, sizeof(str), " boolean %d, menu %s, string \"%s\" ",
+ dev->boolean->cur.val,
+ dev->menu->qmenu[dev->menu->cur.val],
+ dev->string->p_cur.p_char);
+ tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+ snprintf(str, sizeof(str), " integer_menu %lld, value %d ",
+ dev->int_menu->qmenu_int[dev->int_menu->cur.val],
+ dev->int_menu->cur.val);
+ mutex_unlock(dev->ctrl_hdl_user_gen.lock);
+ tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+ if (dev->button_pressed) {
+ dev->button_pressed--;
+ snprintf(str, sizeof(str), " button pressed!");
+ tpg_gen_text(tpg, basep, line++ * line_height, 16, str);
+ }
+ if (dev->osd[0]) {
+ if (vivid_is_hdmi_cap(dev)) {
+ snprintf(str, sizeof(str),
+ " OSD \"%s\"", dev->osd);
+ tpg_gen_text(tpg, basep, line++ * line_height,
+ 16, str);
+ }
+ if (dev->osd_jiffies &&
+ time_is_before_jiffies(dev->osd_jiffies + 5 * HZ)) {
+ dev->osd[0] = 0;
+ dev->osd_jiffies = 0;
+ }
+ }
+ }
+}
+
+/*
+ * Return true if this pixel coordinate is a valid video pixel.
+ */
+static bool valid_pix(struct vivid_dev *dev, int win_y, int win_x, int fb_y, int fb_x)
+{
+ int i;
+
+ if (dev->bitmap_cap) {
+ /*
+ * Only if the corresponding bit in the bitmap is set can
+ * the video pixel be shown. Coordinates are relative to
+ * the overlay window set by VIDIOC_S_FMT.
+ */
+ const u8 *p = dev->bitmap_cap;
+ unsigned stride = (dev->compose_cap.width + 7) / 8;
+
+ if (!(p[stride * win_y + win_x / 8] & (1 << (win_x & 7))))
+ return false;
+ }
+
+ for (i = 0; i < dev->clipcount_cap; i++) {
+ /*
+ * Only if the framebuffer coordinate is not in any of the
+ * clip rectangles will be video pixel be shown.
+ */
+ struct v4l2_rect *r = &dev->clips_cap[i].c;
+
+ if (fb_y >= r->top && fb_y < r->top + r->height &&
+ fb_x >= r->left && fb_x < r->left + r->width)
+ return false;
+ }
+ return true;
+}
+
+/*
+ * Draw the image into the overlay buffer.
+ * Note that the combination of overlay and multiplanar is not supported.
+ */
+static void vivid_overlay(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+ struct tpg_data *tpg = &dev->tpg;
+ unsigned pixsize = tpg_g_twopixelsize(tpg, 0) / 2;
+ void *vbase = dev->fb_vbase_cap;
+ void *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+ unsigned img_width = dev->compose_cap.width;
+ unsigned img_height = dev->compose_cap.height;
+ unsigned stride = tpg->bytesperline[0];
+ /* if quick is true, then valid_pix() doesn't have to be called */
+ bool quick = dev->bitmap_cap == NULL && dev->clipcount_cap == 0;
+ int x, y, w, out_x = 0;
+
+ /*
+ * Overlay support is only supported for formats that have a twopixelsize
+ * that's >= 2. Warn and bail out if that's not the case.
+ */
+ if (WARN_ON(pixsize == 0))
+ return;
+ if ((dev->overlay_cap_field == V4L2_FIELD_TOP ||
+ dev->overlay_cap_field == V4L2_FIELD_BOTTOM) &&
+ dev->overlay_cap_field != buf->vb.field)
+ return;
+
+ vbuf += dev->compose_cap.left * pixsize + dev->compose_cap.top * stride;
+ x = dev->overlay_cap_left;
+ w = img_width;
+ if (x < 0) {
+ out_x = -x;
+ w = w - out_x;
+ x = 0;
+ } else {
+ w = dev->fb_cap.fmt.width - x;
+ if (w > img_width)
+ w = img_width;
+ }
+ if (w <= 0)
+ return;
+ if (dev->overlay_cap_top >= 0)
+ vbase += dev->overlay_cap_top * dev->fb_cap.fmt.bytesperline;
+ for (y = dev->overlay_cap_top;
+ y < dev->overlay_cap_top + (int)img_height;
+ y++, vbuf += stride) {
+ int px;
+
+ if (y < 0 || y > dev->fb_cap.fmt.height)
+ continue;
+ if (quick) {
+ memcpy(vbase + x * pixsize,
+ vbuf + out_x * pixsize, w * pixsize);
+ vbase += dev->fb_cap.fmt.bytesperline;
+ continue;
+ }
+ for (px = 0; px < w; px++) {
+ if (!valid_pix(dev, y - dev->overlay_cap_top,
+ px + out_x, y, px + x))
+ continue;
+ memcpy(vbase + (px + x) * pixsize,
+ vbuf + (px + out_x) * pixsize,
+ pixsize);
+ }
+ vbase += dev->fb_cap.fmt.bytesperline;
+ }
+}
+
+static void vivid_cap_update_frame_period(struct vivid_dev *dev)
+{
+ u64 f_period;
+
+ f_period = (u64)dev->timeperframe_vid_cap.numerator * 1000000000;
+ if (WARN_ON(dev->timeperframe_vid_cap.denominator == 0))
+ dev->timeperframe_vid_cap.denominator = 1;
+ do_div(f_period, dev->timeperframe_vid_cap.denominator);
+ if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+ f_period >>= 1;
+ /*
+ * If "End of Frame", then offset the exposure time by 0.9
+ * of the frame period.
+ */
+ dev->cap_frame_eof_offset = f_period * 9;
+ do_div(dev->cap_frame_eof_offset, 10);
+ dev->cap_frame_period = f_period;
+}
+
+static noinline_for_stack void vivid_thread_vid_cap_tick(struct vivid_dev *dev,
+ int dropped_bufs)
+{
+ struct vivid_buffer *vid_cap_buf = NULL;
+ struct vivid_buffer *vbi_cap_buf = NULL;
+ struct vivid_buffer *meta_cap_buf = NULL;
+ u64 f_time = 0;
+
+ dprintk(dev, 1, "Video Capture Thread Tick\n");
+
+ while (dropped_bufs-- > 1)
+ tpg_update_mv_count(&dev->tpg,
+ dev->field_cap == V4L2_FIELD_NONE ||
+ dev->field_cap == V4L2_FIELD_ALTERNATE);
+
+ /* Drop a certain percentage of buffers. */
+ if (dev->perc_dropped_buffers &&
+ prandom_u32_max(100) < dev->perc_dropped_buffers)
+ goto update_mv;
+
+ spin_lock(&dev->slock);
+ if (!list_empty(&dev->vid_cap_active)) {
+ vid_cap_buf = list_entry(dev->vid_cap_active.next, struct vivid_buffer, list);
+ list_del(&vid_cap_buf->list);
+ }
+ if (!list_empty(&dev->vbi_cap_active)) {
+ if (dev->field_cap != V4L2_FIELD_ALTERNATE ||
+ (dev->vbi_cap_seq_count & 1)) {
+ vbi_cap_buf = list_entry(dev->vbi_cap_active.next,
+ struct vivid_buffer, list);
+ list_del(&vbi_cap_buf->list);
+ }
+ }
+ if (!list_empty(&dev->meta_cap_active)) {
+ meta_cap_buf = list_entry(dev->meta_cap_active.next,
+ struct vivid_buffer, list);
+ list_del(&meta_cap_buf->list);
+ }
+
+ spin_unlock(&dev->slock);
+
+ if (!vid_cap_buf && !vbi_cap_buf && !meta_cap_buf)
+ goto update_mv;
+
+ f_time = dev->cap_frame_period * dev->vid_cap_seq_count +
+ dev->cap_stream_start + dev->time_wrap_offset;
+
+ if (vid_cap_buf) {
+ v4l2_ctrl_request_setup(vid_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vid_cap);
+ /* Fill buffer */
+ vivid_fillbuff(dev, vid_cap_buf);
+ dprintk(dev, 1, "filled buffer %d\n",
+ vid_cap_buf->vb.vb2_buf.index);
+
+ /* Handle overlay */
+ if (dev->overlay_cap_owner && dev->fb_cap.base &&
+ dev->fb_cap.fmt.pixelformat == dev->fmt_cap->fourcc)
+ vivid_overlay(dev, vid_cap_buf);
+
+ v4l2_ctrl_request_complete(vid_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vid_cap);
+ vb2_buffer_done(&vid_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
+ VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+ dprintk(dev, 2, "vid_cap buffer %d done\n",
+ vid_cap_buf->vb.vb2_buf.index);
+
+ vid_cap_buf->vb.vb2_buf.timestamp = f_time;
+ if (!dev->tstamp_src_is_soe)
+ vid_cap_buf->vb.vb2_buf.timestamp += dev->cap_frame_eof_offset;
+ }
+
+ if (vbi_cap_buf) {
+ u64 vbi_period;
+
+ v4l2_ctrl_request_setup(vbi_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vbi_cap);
+ if (dev->stream_sliced_vbi_cap)
+ vivid_sliced_vbi_cap_process(dev, vbi_cap_buf);
+ else
+ vivid_raw_vbi_cap_process(dev, vbi_cap_buf);
+ v4l2_ctrl_request_complete(vbi_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vbi_cap);
+ vb2_buffer_done(&vbi_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
+ VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+ dprintk(dev, 2, "vbi_cap %d done\n",
+ vbi_cap_buf->vb.vb2_buf.index);
+
+ /* If capturing a VBI, offset by 0.05 */
+ vbi_period = dev->cap_frame_period * 5;
+ do_div(vbi_period, 100);
+ vbi_cap_buf->vb.vb2_buf.timestamp = f_time + dev->cap_frame_eof_offset + vbi_period;
+ }
+
+ if (meta_cap_buf) {
+ v4l2_ctrl_request_setup(meta_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_meta_cap);
+ vivid_meta_cap_fillbuff(dev, meta_cap_buf, f_time);
+ v4l2_ctrl_request_complete(meta_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_meta_cap);
+ vb2_buffer_done(&meta_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
+ VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+ dprintk(dev, 2, "meta_cap %d done\n",
+ meta_cap_buf->vb.vb2_buf.index);
+ meta_cap_buf->vb.vb2_buf.timestamp = f_time + dev->cap_frame_eof_offset;
+ }
+
+ dev->dqbuf_error = false;
+
+update_mv:
+ /* Update the test pattern movement counters */
+ tpg_update_mv_count(&dev->tpg, dev->field_cap == V4L2_FIELD_NONE ||
+ dev->field_cap == V4L2_FIELD_ALTERNATE);
+}
+
+static int vivid_thread_vid_cap(void *data)
+{
+ struct vivid_dev *dev = data;
+ u64 numerators_since_start;
+ u64 buffers_since_start;
+ u64 next_jiffies_since_start;
+ unsigned long jiffies_since_start;
+ unsigned long cur_jiffies;
+ unsigned wait_jiffies;
+ unsigned numerator;
+ unsigned denominator;
+ int dropped_bufs;
+
+ dprintk(dev, 1, "Video Capture Thread Start\n");
+
+ set_freezable();
+
+ /* Resets frame counters */
+ dev->cap_seq_offset = 0;
+ dev->cap_seq_count = 0;
+ dev->cap_seq_resync = false;
+ dev->jiffies_vid_cap = jiffies;
+ dev->cap_stream_start = ktime_get_ns();
+ vivid_cap_update_frame_period(dev);
+
+ for (;;) {
+ try_to_freeze();
+ if (kthread_should_stop())
+ break;
+
+ if (!mutex_trylock(&dev->mutex)) {
+ schedule_timeout_uninterruptible(1);
+ continue;
+ }
+
+ cur_jiffies = jiffies;
+ if (dev->cap_seq_resync) {
+ dev->jiffies_vid_cap = cur_jiffies;
+ dev->cap_seq_offset = dev->cap_seq_count + 1;
+ dev->cap_seq_count = 0;
+ dev->cap_stream_start += dev->cap_frame_period *
+ dev->cap_seq_offset;
+ vivid_cap_update_frame_period(dev);
+ dev->cap_seq_resync = false;
+ }
+ numerator = dev->timeperframe_vid_cap.numerator;
+ denominator = dev->timeperframe_vid_cap.denominator;
+
+ if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+ denominator *= 2;
+
+ /* Calculate the number of jiffies since we started streaming */
+ jiffies_since_start = cur_jiffies - dev->jiffies_vid_cap;
+ /* Get the number of buffers streamed since the start */
+ buffers_since_start = (u64)jiffies_since_start * denominator +
+ (HZ * numerator) / 2;
+ do_div(buffers_since_start, HZ * numerator);
+
+ /*
+ * After more than 0xf0000000 (rounded down to a multiple of
+ * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+ * jiffies have passed since we started streaming reset the
+ * counters and keep track of the sequence offset.
+ */
+ if (jiffies_since_start > JIFFIES_RESYNC) {
+ dev->jiffies_vid_cap = cur_jiffies;
+ dev->cap_seq_offset = buffers_since_start;
+ buffers_since_start = 0;
+ }
+ dropped_bufs = buffers_since_start + dev->cap_seq_offset - dev->cap_seq_count;
+ dev->cap_seq_count = buffers_since_start + dev->cap_seq_offset;
+ dev->vid_cap_seq_count = dev->cap_seq_count - dev->vid_cap_seq_start;
+ dev->vbi_cap_seq_count = dev->cap_seq_count - dev->vbi_cap_seq_start;
+ dev->meta_cap_seq_count = dev->cap_seq_count - dev->meta_cap_seq_start;
+
+ vivid_thread_vid_cap_tick(dev, dropped_bufs);
+
+ /*
+ * Calculate the number of 'numerators' streamed since we started,
+ * including the current buffer.
+ */
+ numerators_since_start = ++buffers_since_start * numerator;
+
+ /* And the number of jiffies since we started */
+ jiffies_since_start = jiffies - dev->jiffies_vid_cap;
+
+ mutex_unlock(&dev->mutex);
+
+ /*
+ * Calculate when that next buffer is supposed to start
+ * in jiffies since we started streaming.
+ */
+ next_jiffies_since_start = numerators_since_start * HZ +
+ denominator / 2;
+ do_div(next_jiffies_since_start, denominator);
+ /* If it is in the past, then just schedule asap */
+ if (next_jiffies_since_start < jiffies_since_start)
+ next_jiffies_since_start = jiffies_since_start;
+
+ wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+ schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+ }
+ dprintk(dev, 1, "Video Capture Thread End\n");
+ return 0;
+}
+
+static void vivid_grab_controls(struct vivid_dev *dev, bool grab)
+{
+ v4l2_ctrl_grab(dev->ctrl_has_crop_cap, grab);
+ v4l2_ctrl_grab(dev->ctrl_has_compose_cap, grab);
+ v4l2_ctrl_grab(dev->ctrl_has_scaler_cap, grab);
+}
+
+int vivid_start_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming)
+{
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->kthread_vid_cap) {
+ u32 seq_count = dev->cap_seq_count + dev->seq_wrap * 128;
+
+ if (pstreaming == &dev->vid_cap_streaming)
+ dev->vid_cap_seq_start = seq_count;
+ else if (pstreaming == &dev->vbi_cap_streaming)
+ dev->vbi_cap_seq_start = seq_count;
+ else
+ dev->meta_cap_seq_start = seq_count;
+ *pstreaming = true;
+ return 0;
+ }
+
+ /* Resets frame counters */
+ tpg_init_mv_count(&dev->tpg);
+
+ dev->vid_cap_seq_start = dev->seq_wrap * 128;
+ dev->vbi_cap_seq_start = dev->seq_wrap * 128;
+ dev->meta_cap_seq_start = dev->seq_wrap * 128;
+
+ dev->kthread_vid_cap = kthread_run(vivid_thread_vid_cap, dev,
+ "%s-vid-cap", dev->v4l2_dev.name);
+
+ if (IS_ERR(dev->kthread_vid_cap)) {
+ int err = PTR_ERR(dev->kthread_vid_cap);
+
+ dev->kthread_vid_cap = NULL;
+ v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+ return err;
+ }
+ *pstreaming = true;
+ vivid_grab_controls(dev, true);
+
+ dprintk(dev, 1, "returning from %s\n", __func__);
+ return 0;
+}
+
+void vivid_stop_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming)
+{
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->kthread_vid_cap == NULL)
+ return;
+
+ *pstreaming = false;
+ if (pstreaming == &dev->vid_cap_streaming) {
+ /* Release all active buffers */
+ while (!list_empty(&dev->vid_cap_active)) {
+ struct vivid_buffer *buf;
+
+ buf = list_entry(dev->vid_cap_active.next,
+ struct vivid_buffer, list);
+ list_del(&buf->list);
+ v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vid_cap);
+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+ dprintk(dev, 2, "vid_cap buffer %d done\n",
+ buf->vb.vb2_buf.index);
+ }
+ }
+
+ if (pstreaming == &dev->vbi_cap_streaming) {
+ while (!list_empty(&dev->vbi_cap_active)) {
+ struct vivid_buffer *buf;
+
+ buf = list_entry(dev->vbi_cap_active.next,
+ struct vivid_buffer, list);
+ list_del(&buf->list);
+ v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vbi_cap);
+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+ dprintk(dev, 2, "vbi_cap buffer %d done\n",
+ buf->vb.vb2_buf.index);
+ }
+ }
+
+ if (pstreaming == &dev->meta_cap_streaming) {
+ while (!list_empty(&dev->meta_cap_active)) {
+ struct vivid_buffer *buf;
+
+ buf = list_entry(dev->meta_cap_active.next,
+ struct vivid_buffer, list);
+ list_del(&buf->list);
+ v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_meta_cap);
+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+ dprintk(dev, 2, "meta_cap buffer %d done\n",
+ buf->vb.vb2_buf.index);
+ }
+ }
+
+ if (dev->vid_cap_streaming || dev->vbi_cap_streaming ||
+ dev->meta_cap_streaming)
+ return;
+
+ /* shutdown control thread */
+ vivid_grab_controls(dev, false);
+ kthread_stop(dev->kthread_vid_cap);
+ dev->kthread_vid_cap = NULL;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-kthread-cap.h b/drivers/media/test-drivers/vivid/vivid-kthread-cap.h
new file mode 100644
index 0000000..0f43015
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-kthread-cap.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-kthread-cap.h - video/vbi capture thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_KTHREAD_CAP_H_
+#define _VIVID_KTHREAD_CAP_H_
+
+int vivid_start_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming);
+void vivid_stop_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-kthread-out.c b/drivers/media/test-drivers/vivid/vivid-kthread-out.c
new file mode 100644
index 0000000..6780687
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-kthread-out.c
@@ -0,0 +1,353 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-kthread-out.h - video/vbi output thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/random.h>
+#include <linux/v4l2-dv-timings.h>
+#include <asm/div64.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-rx.h"
+#include "vivid-radio-tx.h"
+#include "vivid-sdr-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-out.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+#include "vivid-kthread-out.h"
+#include "vivid-meta-out.h"
+
+static void vivid_thread_vid_out_tick(struct vivid_dev *dev)
+{
+ struct vivid_buffer *vid_out_buf = NULL;
+ struct vivid_buffer *vbi_out_buf = NULL;
+ struct vivid_buffer *meta_out_buf = NULL;
+
+ dprintk(dev, 1, "Video Output Thread Tick\n");
+
+ /* Drop a certain percentage of buffers. */
+ if (dev->perc_dropped_buffers &&
+ prandom_u32_max(100) < dev->perc_dropped_buffers)
+ return;
+
+ spin_lock(&dev->slock);
+ /*
+ * Only dequeue buffer if there is at least one more pending.
+ * This makes video loopback possible.
+ */
+ if (!list_empty(&dev->vid_out_active) &&
+ !list_is_singular(&dev->vid_out_active)) {
+ vid_out_buf = list_entry(dev->vid_out_active.next,
+ struct vivid_buffer, list);
+ list_del(&vid_out_buf->list);
+ }
+ if (!list_empty(&dev->vbi_out_active) &&
+ (dev->field_out != V4L2_FIELD_ALTERNATE ||
+ (dev->vbi_out_seq_count & 1))) {
+ vbi_out_buf = list_entry(dev->vbi_out_active.next,
+ struct vivid_buffer, list);
+ list_del(&vbi_out_buf->list);
+ }
+ if (!list_empty(&dev->meta_out_active)) {
+ meta_out_buf = list_entry(dev->meta_out_active.next,
+ struct vivid_buffer, list);
+ list_del(&meta_out_buf->list);
+ }
+ spin_unlock(&dev->slock);
+
+ if (!vid_out_buf && !vbi_out_buf && !meta_out_buf)
+ return;
+
+ if (vid_out_buf) {
+ v4l2_ctrl_request_setup(vid_out_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vid_out);
+ v4l2_ctrl_request_complete(vid_out_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vid_out);
+ vid_out_buf->vb.sequence = dev->vid_out_seq_count;
+ if (dev->field_out == V4L2_FIELD_ALTERNATE) {
+ /*
+ * The sequence counter counts frames, not fields.
+ * So divide by two.
+ */
+ vid_out_buf->vb.sequence /= 2;
+ }
+ vid_out_buf->vb.vb2_buf.timestamp =
+ ktime_get_ns() + dev->time_wrap_offset;
+ vb2_buffer_done(&vid_out_buf->vb.vb2_buf, dev->dqbuf_error ?
+ VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+ dprintk(dev, 2, "vid_out buffer %d done\n",
+ vid_out_buf->vb.vb2_buf.index);
+ }
+
+ if (vbi_out_buf) {
+ v4l2_ctrl_request_setup(vbi_out_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vbi_out);
+ v4l2_ctrl_request_complete(vbi_out_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vbi_out);
+ if (dev->stream_sliced_vbi_out)
+ vivid_sliced_vbi_out_process(dev, vbi_out_buf);
+
+ vbi_out_buf->vb.sequence = dev->vbi_out_seq_count;
+ vbi_out_buf->vb.vb2_buf.timestamp =
+ ktime_get_ns() + dev->time_wrap_offset;
+ vb2_buffer_done(&vbi_out_buf->vb.vb2_buf, dev->dqbuf_error ?
+ VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+ dprintk(dev, 2, "vbi_out buffer %d done\n",
+ vbi_out_buf->vb.vb2_buf.index);
+ }
+ if (meta_out_buf) {
+ v4l2_ctrl_request_setup(meta_out_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_meta_out);
+ v4l2_ctrl_request_complete(meta_out_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_meta_out);
+ vivid_meta_out_process(dev, meta_out_buf);
+ meta_out_buf->vb.sequence = dev->meta_out_seq_count;
+ meta_out_buf->vb.vb2_buf.timestamp =
+ ktime_get_ns() + dev->time_wrap_offset;
+ vb2_buffer_done(&meta_out_buf->vb.vb2_buf, dev->dqbuf_error ?
+ VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+ dprintk(dev, 2, "meta_out buffer %d done\n",
+ meta_out_buf->vb.vb2_buf.index);
+ }
+
+ dev->dqbuf_error = false;
+}
+
+static int vivid_thread_vid_out(void *data)
+{
+ struct vivid_dev *dev = data;
+ u64 numerators_since_start;
+ u64 buffers_since_start;
+ u64 next_jiffies_since_start;
+ unsigned long jiffies_since_start;
+ unsigned long cur_jiffies;
+ unsigned wait_jiffies;
+ unsigned numerator;
+ unsigned denominator;
+
+ dprintk(dev, 1, "Video Output Thread Start\n");
+
+ set_freezable();
+
+ /* Resets frame counters */
+ dev->out_seq_offset = 0;
+ if (dev->seq_wrap)
+ dev->out_seq_count = 0xffffff80U;
+ dev->jiffies_vid_out = jiffies;
+ dev->vid_out_seq_start = dev->vbi_out_seq_start = 0;
+ dev->meta_out_seq_start = 0;
+ dev->out_seq_resync = false;
+
+ for (;;) {
+ try_to_freeze();
+ if (kthread_should_stop())
+ break;
+
+ if (!mutex_trylock(&dev->mutex)) {
+ schedule_timeout_uninterruptible(1);
+ continue;
+ }
+
+ cur_jiffies = jiffies;
+ if (dev->out_seq_resync) {
+ dev->jiffies_vid_out = cur_jiffies;
+ dev->out_seq_offset = dev->out_seq_count + 1;
+ dev->out_seq_count = 0;
+ dev->out_seq_resync = false;
+ }
+ numerator = dev->timeperframe_vid_out.numerator;
+ denominator = dev->timeperframe_vid_out.denominator;
+
+ if (dev->field_out == V4L2_FIELD_ALTERNATE)
+ denominator *= 2;
+
+ /* Calculate the number of jiffies since we started streaming */
+ jiffies_since_start = cur_jiffies - dev->jiffies_vid_out;
+ /* Get the number of buffers streamed since the start */
+ buffers_since_start = (u64)jiffies_since_start * denominator +
+ (HZ * numerator) / 2;
+ do_div(buffers_since_start, HZ * numerator);
+
+ /*
+ * After more than 0xf0000000 (rounded down to a multiple of
+ * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+ * jiffies have passed since we started streaming reset the
+ * counters and keep track of the sequence offset.
+ */
+ if (jiffies_since_start > JIFFIES_RESYNC) {
+ dev->jiffies_vid_out = cur_jiffies;
+ dev->out_seq_offset = buffers_since_start;
+ buffers_since_start = 0;
+ }
+ dev->out_seq_count = buffers_since_start + dev->out_seq_offset;
+ dev->vid_out_seq_count = dev->out_seq_count - dev->vid_out_seq_start;
+ dev->vbi_out_seq_count = dev->out_seq_count - dev->vbi_out_seq_start;
+ dev->meta_out_seq_count = dev->out_seq_count - dev->meta_out_seq_start;
+
+ vivid_thread_vid_out_tick(dev);
+ mutex_unlock(&dev->mutex);
+
+ /*
+ * Calculate the number of 'numerators' streamed since we started,
+ * not including the current buffer.
+ */
+ numerators_since_start = buffers_since_start * numerator;
+
+ /* And the number of jiffies since we started */
+ jiffies_since_start = jiffies - dev->jiffies_vid_out;
+
+ /* Increase by the 'numerator' of one buffer */
+ numerators_since_start += numerator;
+ /*
+ * Calculate when that next buffer is supposed to start
+ * in jiffies since we started streaming.
+ */
+ next_jiffies_since_start = numerators_since_start * HZ +
+ denominator / 2;
+ do_div(next_jiffies_since_start, denominator);
+ /* If it is in the past, then just schedule asap */
+ if (next_jiffies_since_start < jiffies_since_start)
+ next_jiffies_since_start = jiffies_since_start;
+
+ wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+ schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+ }
+ dprintk(dev, 1, "Video Output Thread End\n");
+ return 0;
+}
+
+static void vivid_grab_controls(struct vivid_dev *dev, bool grab)
+{
+ v4l2_ctrl_grab(dev->ctrl_has_crop_out, grab);
+ v4l2_ctrl_grab(dev->ctrl_has_compose_out, grab);
+ v4l2_ctrl_grab(dev->ctrl_has_scaler_out, grab);
+ v4l2_ctrl_grab(dev->ctrl_tx_mode, grab);
+ v4l2_ctrl_grab(dev->ctrl_tx_rgb_range, grab);
+}
+
+int vivid_start_generating_vid_out(struct vivid_dev *dev, bool *pstreaming)
+{
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->kthread_vid_out) {
+ u32 seq_count = dev->out_seq_count + dev->seq_wrap * 128;
+
+ if (pstreaming == &dev->vid_out_streaming)
+ dev->vid_out_seq_start = seq_count;
+ else if (pstreaming == &dev->vbi_out_streaming)
+ dev->vbi_out_seq_start = seq_count;
+ else
+ dev->meta_out_seq_start = seq_count;
+ *pstreaming = true;
+ return 0;
+ }
+
+ /* Resets frame counters */
+ dev->jiffies_vid_out = jiffies;
+ dev->vid_out_seq_start = dev->seq_wrap * 128;
+ dev->vbi_out_seq_start = dev->seq_wrap * 128;
+ dev->meta_out_seq_start = dev->seq_wrap * 128;
+
+ dev->kthread_vid_out = kthread_run(vivid_thread_vid_out, dev,
+ "%s-vid-out", dev->v4l2_dev.name);
+
+ if (IS_ERR(dev->kthread_vid_out)) {
+ int err = PTR_ERR(dev->kthread_vid_out);
+
+ dev->kthread_vid_out = NULL;
+ v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+ return err;
+ }
+ *pstreaming = true;
+ vivid_grab_controls(dev, true);
+
+ dprintk(dev, 1, "returning from %s\n", __func__);
+ return 0;
+}
+
+void vivid_stop_generating_vid_out(struct vivid_dev *dev, bool *pstreaming)
+{
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->kthread_vid_out == NULL)
+ return;
+
+ *pstreaming = false;
+ if (pstreaming == &dev->vid_out_streaming) {
+ /* Release all active buffers */
+ while (!list_empty(&dev->vid_out_active)) {
+ struct vivid_buffer *buf;
+
+ buf = list_entry(dev->vid_out_active.next,
+ struct vivid_buffer, list);
+ list_del(&buf->list);
+ v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vid_out);
+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+ dprintk(dev, 2, "vid_out buffer %d done\n",
+ buf->vb.vb2_buf.index);
+ }
+ }
+
+ if (pstreaming == &dev->vbi_out_streaming) {
+ while (!list_empty(&dev->vbi_out_active)) {
+ struct vivid_buffer *buf;
+
+ buf = list_entry(dev->vbi_out_active.next,
+ struct vivid_buffer, list);
+ list_del(&buf->list);
+ v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_vbi_out);
+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+ dprintk(dev, 2, "vbi_out buffer %d done\n",
+ buf->vb.vb2_buf.index);
+ }
+ }
+
+ if (pstreaming == &dev->meta_out_streaming) {
+ while (!list_empty(&dev->meta_out_active)) {
+ struct vivid_buffer *buf;
+
+ buf = list_entry(dev->meta_out_active.next,
+ struct vivid_buffer, list);
+ list_del(&buf->list);
+ v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_meta_out);
+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+ dprintk(dev, 2, "meta_out buffer %d done\n",
+ buf->vb.vb2_buf.index);
+ }
+ }
+
+ if (dev->vid_out_streaming || dev->vbi_out_streaming ||
+ dev->meta_out_streaming)
+ return;
+
+ /* shutdown control thread */
+ vivid_grab_controls(dev, false);
+ kthread_stop(dev->kthread_vid_out);
+ dev->kthread_vid_out = NULL;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-kthread-out.h b/drivers/media/test-drivers/vivid/vivid-kthread-out.h
new file mode 100644
index 0000000..d5bcf44
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-kthread-out.h
@@ -0,0 +1,14 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-kthread-out.h - video/vbi output thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_KTHREAD_OUT_H_
+#define _VIVID_KTHREAD_OUT_H_
+
+int vivid_start_generating_vid_out(struct vivid_dev *dev, bool *pstreaming);
+void vivid_stop_generating_vid_out(struct vivid_dev *dev, bool *pstreaming);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-kthread-touch.c b/drivers/media/test-drivers/vivid/vivid-kthread-touch.c
new file mode 100644
index 0000000..674507b
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-kthread-touch.c
@@ -0,0 +1,181 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-kthread-touch.c - touch capture thread support functions.
+ *
+ */
+
+#include <linux/freezer.h>
+#include "vivid-core.h"
+#include "vivid-kthread-touch.h"
+#include "vivid-touch-cap.h"
+
+static noinline_for_stack void vivid_thread_tch_cap_tick(struct vivid_dev *dev,
+ int dropped_bufs)
+{
+ struct vivid_buffer *tch_cap_buf = NULL;
+
+ spin_lock(&dev->slock);
+ if (!list_empty(&dev->touch_cap_active)) {
+ tch_cap_buf = list_entry(dev->touch_cap_active.next,
+ struct vivid_buffer, list);
+ list_del(&tch_cap_buf->list);
+ }
+
+ spin_unlock(&dev->slock);
+
+ if (tch_cap_buf) {
+ v4l2_ctrl_request_setup(tch_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_touch_cap);
+
+ vivid_fillbuff_tch(dev, tch_cap_buf);
+ v4l2_ctrl_request_complete(tch_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_touch_cap);
+ vb2_buffer_done(&tch_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
+ VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+ dprintk(dev, 2, "touch_cap buffer %d done\n",
+ tch_cap_buf->vb.vb2_buf.index);
+
+ tch_cap_buf->vb.vb2_buf.timestamp = ktime_get_ns() + dev->time_wrap_offset;
+ }
+ dev->dqbuf_error = false;
+}
+
+static int vivid_thread_touch_cap(void *data)
+{
+ struct vivid_dev *dev = data;
+ u64 numerators_since_start;
+ u64 buffers_since_start;
+ u64 next_jiffies_since_start;
+ unsigned long jiffies_since_start;
+ unsigned long cur_jiffies;
+ unsigned int wait_jiffies;
+ unsigned int numerator;
+ unsigned int denominator;
+ int dropped_bufs;
+
+ dprintk(dev, 1, "Touch Capture Thread Start\n");
+
+ set_freezable();
+
+ /* Resets frame counters */
+ dev->touch_cap_seq_offset = 0;
+ dev->touch_cap_seq_count = 0;
+ dev->touch_cap_seq_resync = false;
+ dev->jiffies_touch_cap = jiffies;
+
+ for (;;) {
+ try_to_freeze();
+ if (kthread_should_stop())
+ break;
+
+ if (!mutex_trylock(&dev->mutex)) {
+ schedule_timeout_uninterruptible(1);
+ continue;
+ }
+ cur_jiffies = jiffies;
+ if (dev->touch_cap_seq_resync) {
+ dev->jiffies_touch_cap = cur_jiffies;
+ dev->touch_cap_seq_offset = dev->touch_cap_seq_count + 1;
+ dev->touch_cap_seq_count = 0;
+ dev->cap_seq_resync = false;
+ }
+ denominator = dev->timeperframe_tch_cap.denominator;
+ numerator = dev->timeperframe_tch_cap.numerator;
+
+ /* Calculate the number of jiffies since we started streaming */
+ jiffies_since_start = cur_jiffies - dev->jiffies_touch_cap;
+ /* Get the number of buffers streamed since the start */
+ buffers_since_start = (u64)jiffies_since_start * denominator +
+ (HZ * numerator) / 2;
+ do_div(buffers_since_start, HZ * numerator);
+
+ /*
+ * After more than 0xf0000000 (rounded down to a multiple of
+ * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+ * jiffies have passed since we started streaming reset the
+ * counters and keep track of the sequence offset.
+ */
+ if (jiffies_since_start > JIFFIES_RESYNC) {
+ dev->jiffies_touch_cap = cur_jiffies;
+ dev->cap_seq_offset = buffers_since_start;
+ buffers_since_start = 0;
+ }
+ dropped_bufs = buffers_since_start + dev->touch_cap_seq_offset - dev->touch_cap_seq_count;
+ dev->touch_cap_seq_count = buffers_since_start + dev->touch_cap_seq_offset;
+
+ vivid_thread_tch_cap_tick(dev, dropped_bufs);
+
+ /*
+ * Calculate the number of 'numerators' streamed
+ * since we started, including the current buffer.
+ */
+ numerators_since_start = ++buffers_since_start * numerator;
+
+ /* And the number of jiffies since we started */
+ jiffies_since_start = jiffies - dev->jiffies_touch_cap;
+
+ mutex_unlock(&dev->mutex);
+
+ /*
+ * Calculate when that next buffer is supposed to start
+ * in jiffies since we started streaming.
+ */
+ next_jiffies_since_start = numerators_since_start * HZ +
+ denominator / 2;
+ do_div(next_jiffies_since_start, denominator);
+ /* If it is in the past, then just schedule asap */
+ if (next_jiffies_since_start < jiffies_since_start)
+ next_jiffies_since_start = jiffies_since_start;
+
+ wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+ schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+ }
+ dprintk(dev, 1, "Touch Capture Thread End\n");
+ return 0;
+}
+
+int vivid_start_generating_touch_cap(struct vivid_dev *dev)
+{
+ if (dev->kthread_touch_cap) {
+ dev->touch_cap_streaming = true;
+ return 0;
+ }
+
+ dev->kthread_touch_cap = kthread_run(vivid_thread_touch_cap, dev,
+ "%s-tch-cap", dev->v4l2_dev.name);
+
+ if (IS_ERR(dev->kthread_touch_cap)) {
+ int err = PTR_ERR(dev->kthread_touch_cap);
+
+ dev->kthread_touch_cap = NULL;
+ v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+ return err;
+ }
+ dev->touch_cap_streaming = true;
+ dprintk(dev, 1, "returning from %s\n", __func__);
+ return 0;
+}
+
+void vivid_stop_generating_touch_cap(struct vivid_dev *dev)
+{
+ if (!dev->kthread_touch_cap)
+ return;
+
+ dev->touch_cap_streaming = false;
+
+ while (!list_empty(&dev->touch_cap_active)) {
+ struct vivid_buffer *buf;
+
+ buf = list_entry(dev->touch_cap_active.next,
+ struct vivid_buffer, list);
+ list_del(&buf->list);
+ v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_touch_cap);
+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+ dprintk(dev, 2, "touch_cap buffer %d done\n",
+ buf->vb.vb2_buf.index);
+ }
+
+ kthread_stop(dev->kthread_touch_cap);
+ dev->kthread_touch_cap = NULL;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-kthread-touch.h b/drivers/media/test-drivers/vivid/vivid-kthread-touch.h
new file mode 100644
index 0000000..ecf79b4
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-kthread-touch.h
@@ -0,0 +1,13 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-kthread-cap.h - video/vbi capture thread support functions.
+ *
+ */
+
+#ifndef _VIVID_KTHREAD_CAP_H_
+#define _VIVID_KTHREAD_CAP_H_
+
+int vivid_start_generating_touch_cap(struct vivid_dev *dev);
+void vivid_stop_generating_touch_cap(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-meta-cap.c b/drivers/media/test-drivers/vivid/vivid-meta-cap.c
new file mode 100644
index 0000000..780f968
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-meta-cap.c
@@ -0,0 +1,201 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-meta-cap.c - meta capture support functions.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <linux/usb/video.h>
+
+#include "vivid-core.h"
+#include "vivid-kthread-cap.h"
+#include "vivid-meta-cap.h"
+
+static int meta_cap_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+ unsigned int *nplanes, unsigned int sizes[],
+ struct device *alloc_devs[])
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ unsigned int size = sizeof(struct vivid_uvc_meta_buf);
+
+ if (!vivid_is_webcam(dev))
+ return -EINVAL;
+
+ if (*nplanes) {
+ if (sizes[0] < size)
+ return -EINVAL;
+ } else {
+ sizes[0] = size;
+ }
+
+ if (vq->num_buffers + *nbuffers < 2)
+ *nbuffers = 2 - vq->num_buffers;
+
+ *nplanes = 1;
+ return 0;
+}
+
+static int meta_cap_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ unsigned int size = sizeof(struct vivid_uvc_meta_buf);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->buf_prepare_error) {
+ /*
+ * Error injection: test what happens if buf_prepare() returns
+ * an error.
+ */
+ dev->buf_prepare_error = false;
+ return -EINVAL;
+ }
+ if (vb2_plane_size(vb, 0) < size) {
+ dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+ __func__, vb2_plane_size(vb, 0), size);
+ return -EINVAL;
+ }
+ vb2_set_plane_payload(vb, 0, size);
+
+ return 0;
+}
+
+static void meta_cap_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ spin_lock(&dev->slock);
+ list_add_tail(&buf->list, &dev->meta_cap_active);
+ spin_unlock(&dev->slock);
+}
+
+static int meta_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ int err;
+
+ dprintk(dev, 1, "%s\n", __func__);
+ dev->meta_cap_seq_count = 0;
+ if (dev->start_streaming_error) {
+ dev->start_streaming_error = false;
+ err = -EINVAL;
+ } else {
+ err = vivid_start_generating_vid_cap(dev,
+ &dev->meta_cap_streaming);
+ }
+ if (err) {
+ struct vivid_buffer *buf, *tmp;
+
+ list_for_each_entry_safe(buf, tmp,
+ &dev->meta_cap_active, list) {
+ list_del(&buf->list);
+ vb2_buffer_done(&buf->vb.vb2_buf,
+ VB2_BUF_STATE_QUEUED);
+ }
+ }
+ return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void meta_cap_stop_streaming(struct vb2_queue *vq)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+ dprintk(dev, 1, "%s\n", __func__);
+ vivid_stop_generating_vid_cap(dev, &dev->meta_cap_streaming);
+}
+
+static void meta_cap_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_meta_cap);
+}
+
+const struct vb2_ops vivid_meta_cap_qops = {
+ .queue_setup = meta_cap_queue_setup,
+ .buf_prepare = meta_cap_buf_prepare,
+ .buf_queue = meta_cap_buf_queue,
+ .start_streaming = meta_cap_start_streaming,
+ .stop_streaming = meta_cap_stop_streaming,
+ .buf_request_complete = meta_cap_buf_request_complete,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+int vidioc_enum_fmt_meta_cap(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_webcam(dev))
+ return -EINVAL;
+
+ if (f->index > 0)
+ return -EINVAL;
+
+ f->type = V4L2_BUF_TYPE_META_CAPTURE;
+ f->pixelformat = V4L2_META_FMT_UVC;
+ return 0;
+}
+
+int vidioc_g_fmt_meta_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_meta_format *meta = &f->fmt.meta;
+
+ if (!vivid_is_webcam(dev) || !dev->has_meta_cap)
+ return -EINVAL;
+
+ meta->dataformat = V4L2_META_FMT_UVC;
+ meta->buffersize = sizeof(struct vivid_uvc_meta_buf);
+ return 0;
+}
+
+void vivid_meta_cap_fillbuff(struct vivid_dev *dev,
+ struct vivid_buffer *buf, u64 soe)
+{
+ struct vivid_uvc_meta_buf *meta = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+ int buf_off = 0;
+
+ buf->vb.sequence = dev->meta_cap_seq_count;
+ if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+ buf->vb.sequence /= 2;
+ memset(meta, 1, vb2_plane_size(&buf->vb.vb2_buf, 0));
+
+ meta->ns = ktime_get_ns();
+ meta->sof = buf->vb.sequence * 30;
+ meta->length = sizeof(*meta) - offsetof(struct vivid_uvc_meta_buf, length);
+ meta->flags = UVC_STREAM_EOH | UVC_STREAM_EOF;
+
+ if ((buf->vb.sequence % 2) == 0)
+ meta->flags |= UVC_STREAM_FID;
+
+ dprintk(dev, 2, "%s ns:%llu sof:%4d len:%u flags: 0x%02x",
+ __func__, meta->ns, meta->sof, meta->length, meta->flags);
+ if (dev->meta_pts) {
+ meta->flags |= UVC_STREAM_PTS;
+ meta->buf[0] = div_u64(soe, VIVID_META_CLOCK_UNIT);
+ buf_off = 4;
+ dprintk(dev, 2, " pts: %u\n", *(__u32 *)(meta->buf));
+ }
+
+ if (dev->meta_scr) {
+ meta->flags |= UVC_STREAM_SCR;
+ meta->buf[buf_off] = div_u64((soe + dev->cap_frame_eof_offset),
+ VIVID_META_CLOCK_UNIT);
+
+ meta->buf[buf_off + 4] = (buf->vb.sequence * 30) % 1000;
+ dprintk(dev, 2, " stc: %u, sof counter: %u\n",
+ *(__u32 *)(meta->buf + buf_off),
+ *(__u16 *)(meta->buf + buf_off + 4));
+ }
+ dprintk(dev, 2, "\n");
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-meta-cap.h b/drivers/media/test-drivers/vivid/vivid-meta-cap.h
new file mode 100644
index 0000000..4670d00
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-meta-cap.h
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-meta-cap.h - meta capture support functions.
+ */
+#ifndef _VIVID_META_CAP_H_
+#define _VIVID_META_CAP_H_
+
+#define VIVID_META_CLOCK_UNIT 10 /* 100 MHz */
+
+struct vivid_uvc_meta_buf {
+ __u64 ns;
+ __u16 sof;
+ __u8 length;
+ __u8 flags;
+ __u8 buf[10]; /* PTS(4)+STC(4)+SOF(2) */
+} __packed;
+
+void vivid_meta_cap_fillbuff(struct vivid_dev *dev,
+ struct vivid_buffer *buf, u64 soe);
+
+int vidioc_enum_fmt_meta_cap(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f);
+
+int vidioc_g_fmt_meta_cap(struct file *file, void *priv,
+ struct v4l2_format *f);
+
+extern const struct vb2_ops vivid_meta_cap_qops;
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-meta-out.c b/drivers/media/test-drivers/vivid/vivid-meta-out.c
new file mode 100644
index 0000000..95835b5
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-meta-out.c
@@ -0,0 +1,175 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-meta-out.c - meta output support functions.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+#include <linux/usb/video.h>
+
+#include "vivid-core.h"
+#include "vivid-kthread-out.h"
+#include "vivid-meta-out.h"
+
+static int meta_out_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+ unsigned int *nplanes, unsigned int sizes[],
+ struct device *alloc_devs[])
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ unsigned int size = sizeof(struct vivid_meta_out_buf);
+
+ if (!vivid_is_webcam(dev))
+ return -EINVAL;
+
+ if (*nplanes) {
+ if (sizes[0] < size)
+ return -EINVAL;
+ } else {
+ sizes[0] = size;
+ }
+
+ if (vq->num_buffers + *nbuffers < 2)
+ *nbuffers = 2 - vq->num_buffers;
+
+ *nplanes = 1;
+ return 0;
+}
+
+static int meta_out_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ unsigned int size = sizeof(struct vivid_meta_out_buf);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->buf_prepare_error) {
+ /*
+ * Error injection: test what happens if buf_prepare() returns
+ * an error.
+ */
+ dev->buf_prepare_error = false;
+ return -EINVAL;
+ }
+ if (vb2_plane_size(vb, 0) < size) {
+ dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+ __func__, vb2_plane_size(vb, 0), size);
+ return -EINVAL;
+ }
+ vb2_set_plane_payload(vb, 0, size);
+
+ return 0;
+}
+
+static void meta_out_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ spin_lock(&dev->slock);
+ list_add_tail(&buf->list, &dev->meta_out_active);
+ spin_unlock(&dev->slock);
+}
+
+static int meta_out_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ int err;
+
+ dprintk(dev, 1, "%s\n", __func__);
+ dev->meta_out_seq_count = 0;
+ if (dev->start_streaming_error) {
+ dev->start_streaming_error = false;
+ err = -EINVAL;
+ } else {
+ err = vivid_start_generating_vid_out(dev,
+ &dev->meta_out_streaming);
+ }
+ if (err) {
+ struct vivid_buffer *buf, *tmp;
+
+ list_for_each_entry_safe(buf, tmp,
+ &dev->meta_out_active, list) {
+ list_del(&buf->list);
+ vb2_buffer_done(&buf->vb.vb2_buf,
+ VB2_BUF_STATE_QUEUED);
+ }
+ }
+ return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void meta_out_stop_streaming(struct vb2_queue *vq)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+ dprintk(dev, 1, "%s\n", __func__);
+ vivid_stop_generating_vid_out(dev, &dev->meta_out_streaming);
+}
+
+static void meta_out_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_meta_out);
+}
+
+const struct vb2_ops vivid_meta_out_qops = {
+ .queue_setup = meta_out_queue_setup,
+ .buf_prepare = meta_out_buf_prepare,
+ .buf_queue = meta_out_buf_queue,
+ .start_streaming = meta_out_start_streaming,
+ .stop_streaming = meta_out_stop_streaming,
+ .buf_request_complete = meta_out_buf_request_complete,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+int vidioc_enum_fmt_meta_out(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_webcam(dev))
+ return -EINVAL;
+
+ if (f->index > 0)
+ return -EINVAL;
+
+ f->type = V4L2_BUF_TYPE_META_OUTPUT;
+ f->pixelformat = V4L2_META_FMT_VIVID;
+ return 0;
+}
+
+int vidioc_g_fmt_meta_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_meta_format *meta = &f->fmt.meta;
+
+ if (!vivid_is_webcam(dev) || !dev->has_meta_out)
+ return -EINVAL;
+
+ meta->dataformat = V4L2_META_FMT_VIVID;
+ meta->buffersize = sizeof(struct vivid_meta_out_buf);
+ return 0;
+}
+
+void vivid_meta_out_process(struct vivid_dev *dev,
+ struct vivid_buffer *buf)
+{
+ struct vivid_meta_out_buf *meta = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+
+ v4l2_ctrl_s_ctrl(dev->brightness, meta->brightness);
+ v4l2_ctrl_s_ctrl(dev->contrast, meta->contrast);
+ v4l2_ctrl_s_ctrl(dev->saturation, meta->saturation);
+ v4l2_ctrl_s_ctrl(dev->hue, meta->hue);
+
+ dprintk(dev, 2, " %s brightness %u contrast %u saturation %u hue %d\n",
+ __func__, meta->brightness, meta->contrast,
+ meta->saturation, meta->hue);
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-meta-out.h b/drivers/media/test-drivers/vivid/vivid-meta-out.h
new file mode 100644
index 0000000..0c639b7
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-meta-out.h
@@ -0,0 +1,25 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-meta-out.h - meta output support functions.
+ */
+#ifndef _VIVID_META_OUT_H_
+#define _VIVID_META_OUT_H_
+
+struct vivid_meta_out_buf {
+ u16 brightness;
+ u16 contrast;
+ u16 saturation;
+ s16 hue;
+};
+
+void vivid_meta_out_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+int vidioc_enum_fmt_meta_out(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_meta_out(struct file *file, void *priv,
+ struct v4l2_format *f);
+int vidioc_s_fmt_meta_out(struct file *file, void *priv,
+ struct v4l2_format *f);
+
+extern const struct vb2_ops vivid_meta_out_qops;
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-osd.c b/drivers/media/test-drivers/vivid/vivid-osd.c
new file mode 100644
index 0000000..fbaec8a
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-osd.c
@@ -0,0 +1,388 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-osd.c - osd support for testing overlays.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/fb.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-osd.h"
+
+#define MAX_OSD_WIDTH 720
+#define MAX_OSD_HEIGHT 576
+
+/*
+ * Order: white, yellow, cyan, green, magenta, red, blue, black,
+ * and same again with the alpha bit set (if any)
+ */
+static const u16 rgb555[16] = {
+ 0x7fff, 0x7fe0, 0x03ff, 0x03e0, 0x7c1f, 0x7c00, 0x001f, 0x0000,
+ 0xffff, 0xffe0, 0x83ff, 0x83e0, 0xfc1f, 0xfc00, 0x801f, 0x8000
+};
+
+static const u16 rgb565[16] = {
+ 0xffff, 0xffe0, 0x07ff, 0x07e0, 0xf81f, 0xf800, 0x001f, 0x0000,
+ 0xffff, 0xffe0, 0x07ff, 0x07e0, 0xf81f, 0xf800, 0x001f, 0x0000
+};
+
+void vivid_clear_fb(struct vivid_dev *dev)
+{
+ void *p = dev->video_vbase;
+ const u16 *rgb = rgb555;
+ unsigned x, y;
+
+ if (dev->fb_defined.green.length == 6)
+ rgb = rgb565;
+
+ for (y = 0; y < dev->display_height; y++) {
+ u16 *d = p;
+
+ for (x = 0; x < dev->display_width; x++)
+ d[x] = rgb[(y / 16 + x / 16) % 16];
+ p += dev->display_byte_stride;
+ }
+}
+
+/* --------------------------------------------------------------------- */
+
+static int vivid_fb_ioctl(struct fb_info *info, unsigned cmd, unsigned long arg)
+{
+ struct vivid_dev *dev = (struct vivid_dev *)info->par;
+
+ switch (cmd) {
+ case FBIOGET_VBLANK: {
+ struct fb_vblank vblank;
+
+ memset(&vblank, 0, sizeof(vblank));
+ vblank.flags = FB_VBLANK_HAVE_COUNT | FB_VBLANK_HAVE_VCOUNT |
+ FB_VBLANK_HAVE_VSYNC;
+ vblank.count = 0;
+ vblank.vcount = 0;
+ vblank.hcount = 0;
+ if (copy_to_user((void __user *)arg, &vblank, sizeof(vblank)))
+ return -EFAULT;
+ return 0;
+ }
+
+ default:
+ dprintk(dev, 1, "Unknown ioctl %08x\n", cmd);
+ return -EINVAL;
+ }
+ return 0;
+}
+
+/* Framebuffer device handling */
+
+static int vivid_fb_set_var(struct vivid_dev *dev, struct fb_var_screeninfo *var)
+{
+ dprintk(dev, 1, "vivid_fb_set_var\n");
+
+ if (var->bits_per_pixel != 16) {
+ dprintk(dev, 1, "vivid_fb_set_var - Invalid bpp\n");
+ return -EINVAL;
+ }
+ dev->display_byte_stride = var->xres * dev->bytes_per_pixel;
+
+ return 0;
+}
+
+static int vivid_fb_get_fix(struct vivid_dev *dev, struct fb_fix_screeninfo *fix)
+{
+ dprintk(dev, 1, "vivid_fb_get_fix\n");
+ memset(fix, 0, sizeof(struct fb_fix_screeninfo));
+ strscpy(fix->id, "vioverlay fb", sizeof(fix->id));
+ fix->smem_start = dev->video_pbase;
+ fix->smem_len = dev->video_buffer_size;
+ fix->type = FB_TYPE_PACKED_PIXELS;
+ fix->visual = FB_VISUAL_TRUECOLOR;
+ fix->xpanstep = 1;
+ fix->ypanstep = 1;
+ fix->ywrapstep = 0;
+ fix->line_length = dev->display_byte_stride;
+ fix->accel = FB_ACCEL_NONE;
+ return 0;
+}
+
+/* Check the requested display mode, returning -EINVAL if we can't
+ handle it. */
+
+static int _vivid_fb_check_var(struct fb_var_screeninfo *var, struct vivid_dev *dev)
+{
+ dprintk(dev, 1, "vivid_fb_check_var\n");
+
+ var->bits_per_pixel = 16;
+ if (var->green.length == 5) {
+ var->red.offset = 10;
+ var->red.length = 5;
+ var->green.offset = 5;
+ var->green.length = 5;
+ var->blue.offset = 0;
+ var->blue.length = 5;
+ var->transp.offset = 15;
+ var->transp.length = 1;
+ } else {
+ var->red.offset = 11;
+ var->red.length = 5;
+ var->green.offset = 5;
+ var->green.length = 6;
+ var->blue.offset = 0;
+ var->blue.length = 5;
+ var->transp.offset = 0;
+ var->transp.length = 0;
+ }
+ var->xoffset = var->yoffset = 0;
+ var->left_margin = var->upper_margin = 0;
+ var->nonstd = 0;
+
+ var->vmode &= ~FB_VMODE_MASK;
+ var->vmode |= FB_VMODE_NONINTERLACED;
+
+ /* Dummy values */
+ var->hsync_len = 24;
+ var->vsync_len = 2;
+ var->pixclock = 84316;
+ var->right_margin = 776;
+ var->lower_margin = 591;
+ return 0;
+}
+
+static int vivid_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+ struct vivid_dev *dev = (struct vivid_dev *) info->par;
+
+ dprintk(dev, 1, "vivid_fb_check_var\n");
+ return _vivid_fb_check_var(var, dev);
+}
+
+static int vivid_fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+ return 0;
+}
+
+static int vivid_fb_set_par(struct fb_info *info)
+{
+ int rc = 0;
+ struct vivid_dev *dev = (struct vivid_dev *) info->par;
+
+ dprintk(dev, 1, "vivid_fb_set_par\n");
+
+ rc = vivid_fb_set_var(dev, &info->var);
+ vivid_fb_get_fix(dev, &info->fix);
+ return rc;
+}
+
+static int vivid_fb_setcolreg(unsigned regno, unsigned red, unsigned green,
+ unsigned blue, unsigned transp,
+ struct fb_info *info)
+{
+ u32 color, *palette;
+
+ if (regno >= info->cmap.len)
+ return -EINVAL;
+
+ color = ((transp & 0xFF00) << 16) | ((red & 0xFF00) << 8) |
+ (green & 0xFF00) | ((blue & 0xFF00) >> 8);
+ if (regno >= 16)
+ return -EINVAL;
+
+ palette = info->pseudo_palette;
+ if (info->var.bits_per_pixel == 16) {
+ switch (info->var.green.length) {
+ case 6:
+ color = (red & 0xf800) |
+ ((green & 0xfc00) >> 5) |
+ ((blue & 0xf800) >> 11);
+ break;
+ case 5:
+ color = ((red & 0xf800) >> 1) |
+ ((green & 0xf800) >> 6) |
+ ((blue & 0xf800) >> 11) |
+ (transp ? 0x8000 : 0);
+ break;
+ }
+ }
+ palette[regno] = color;
+ return 0;
+}
+
+/* We don't really support blanking. All this does is enable or
+ disable the OSD. */
+static int vivid_fb_blank(int blank_mode, struct fb_info *info)
+{
+ struct vivid_dev *dev = (struct vivid_dev *)info->par;
+
+ dprintk(dev, 1, "Set blanking mode : %d\n", blank_mode);
+ switch (blank_mode) {
+ case FB_BLANK_UNBLANK:
+ break;
+ case FB_BLANK_NORMAL:
+ case FB_BLANK_HSYNC_SUSPEND:
+ case FB_BLANK_VSYNC_SUSPEND:
+ case FB_BLANK_POWERDOWN:
+ break;
+ }
+ return 0;
+}
+
+static const struct fb_ops vivid_fb_ops = {
+ .owner = THIS_MODULE,
+ .fb_check_var = vivid_fb_check_var,
+ .fb_set_par = vivid_fb_set_par,
+ .fb_setcolreg = vivid_fb_setcolreg,
+ .fb_fillrect = cfb_fillrect,
+ .fb_copyarea = cfb_copyarea,
+ .fb_imageblit = cfb_imageblit,
+ .fb_cursor = NULL,
+ .fb_ioctl = vivid_fb_ioctl,
+ .fb_pan_display = vivid_fb_pan_display,
+ .fb_blank = vivid_fb_blank,
+};
+
+/* Initialization */
+
+
+/* Setup our initial video mode */
+static int vivid_fb_init_vidmode(struct vivid_dev *dev)
+{
+ struct v4l2_rect start_window;
+
+ /* Color mode */
+
+ dev->bits_per_pixel = 16;
+ dev->bytes_per_pixel = dev->bits_per_pixel / 8;
+
+ start_window.width = MAX_OSD_WIDTH;
+ start_window.left = 0;
+
+ dev->display_byte_stride = start_window.width * dev->bytes_per_pixel;
+
+ /* Vertical size & position */
+
+ start_window.height = MAX_OSD_HEIGHT;
+ start_window.top = 0;
+
+ dev->display_width = start_window.width;
+ dev->display_height = start_window.height;
+
+ /* Generate a valid fb_var_screeninfo */
+
+ dev->fb_defined.xres = dev->display_width;
+ dev->fb_defined.yres = dev->display_height;
+ dev->fb_defined.xres_virtual = dev->display_width;
+ dev->fb_defined.yres_virtual = dev->display_height;
+ dev->fb_defined.bits_per_pixel = dev->bits_per_pixel;
+ dev->fb_defined.vmode = FB_VMODE_NONINTERLACED;
+ dev->fb_defined.left_margin = start_window.left + 1;
+ dev->fb_defined.upper_margin = start_window.top + 1;
+ dev->fb_defined.accel_flags = FB_ACCEL_NONE;
+ dev->fb_defined.nonstd = 0;
+ /* set default to 1:5:5:5 */
+ dev->fb_defined.green.length = 5;
+
+ /* We've filled in the most data, let the usual mode check
+ routine fill in the rest. */
+ _vivid_fb_check_var(&dev->fb_defined, dev);
+
+ /* Generate valid fb_fix_screeninfo */
+
+ vivid_fb_get_fix(dev, &dev->fb_fix);
+
+ /* Generate valid fb_info */
+
+ dev->fb_info.node = -1;
+ dev->fb_info.flags = FBINFO_FLAG_DEFAULT;
+ dev->fb_info.par = dev;
+ dev->fb_info.var = dev->fb_defined;
+ dev->fb_info.fix = dev->fb_fix;
+ dev->fb_info.screen_base = (u8 __iomem *)dev->video_vbase;
+ dev->fb_info.fbops = &vivid_fb_ops;
+
+ /* Supply some monitor specs. Bogus values will do for now */
+ dev->fb_info.monspecs.hfmin = 8000;
+ dev->fb_info.monspecs.hfmax = 70000;
+ dev->fb_info.monspecs.vfmin = 10;
+ dev->fb_info.monspecs.vfmax = 100;
+
+ /* Allocate color map */
+ if (fb_alloc_cmap(&dev->fb_info.cmap, 256, 1)) {
+ pr_err("abort, unable to alloc cmap\n");
+ return -ENOMEM;
+ }
+
+ /* Allocate the pseudo palette */
+ dev->fb_info.pseudo_palette = kmalloc_array(16, sizeof(u32), GFP_KERNEL);
+
+ return dev->fb_info.pseudo_palette ? 0 : -ENOMEM;
+}
+
+/* Release any memory we've grabbed */
+void vivid_fb_release_buffers(struct vivid_dev *dev)
+{
+ if (dev->video_vbase == NULL)
+ return;
+
+ /* Release cmap */
+ if (dev->fb_info.cmap.len)
+ fb_dealloc_cmap(&dev->fb_info.cmap);
+
+ /* Release pseudo palette */
+ kfree(dev->fb_info.pseudo_palette);
+ kfree(dev->video_vbase);
+}
+
+/* Initialize the specified card */
+
+int vivid_fb_init(struct vivid_dev *dev)
+{
+ int ret;
+
+ dev->video_buffer_size = MAX_OSD_HEIGHT * MAX_OSD_WIDTH * 2;
+ dev->video_vbase = kzalloc(dev->video_buffer_size, GFP_KERNEL | GFP_DMA32);
+ if (dev->video_vbase == NULL)
+ return -ENOMEM;
+ dev->video_pbase = virt_to_phys(dev->video_vbase);
+
+ pr_info("Framebuffer at 0x%lx, mapped to 0x%p, size %dk\n",
+ dev->video_pbase, dev->video_vbase,
+ dev->video_buffer_size / 1024);
+
+ /* Set the startup video mode information */
+ ret = vivid_fb_init_vidmode(dev);
+ if (ret) {
+ vivid_fb_release_buffers(dev);
+ return ret;
+ }
+
+ vivid_clear_fb(dev);
+
+ /* Register the framebuffer */
+ if (register_framebuffer(&dev->fb_info) < 0) {
+ vivid_fb_release_buffers(dev);
+ return -EINVAL;
+ }
+
+ /* Set the card to the requested mode */
+ vivid_fb_set_par(&dev->fb_info);
+ return 0;
+
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-osd.h b/drivers/media/test-drivers/vivid/vivid-osd.h
new file mode 100644
index 0000000..f9ac1af
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-osd.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-osd.h - output overlay support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_OSD_H_
+#define _VIVID_OSD_H_
+
+int vivid_fb_init(struct vivid_dev *dev);
+void vivid_fb_release_buffers(struct vivid_dev *dev);
+void vivid_clear_fb(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-radio-common.c b/drivers/media/test-drivers/vivid/vivid-radio-common.c
new file mode 100644
index 0000000..138c7bc
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-radio-common.c
@@ -0,0 +1,177 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-radio-common.c - common radio rx/tx support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-radio-common.h"
+#include "vivid-rds-gen.h"
+
+/*
+ * These functions are shared between the vivid receiver and transmitter
+ * since both use the same frequency bands.
+ */
+
+const struct v4l2_frequency_band vivid_radio_bands[TOT_BANDS] = {
+ /* Band FM */
+ {
+ .type = V4L2_TUNER_RADIO,
+ .index = 0,
+ .capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+ V4L2_TUNER_CAP_FREQ_BANDS,
+ .rangelow = FM_FREQ_RANGE_LOW,
+ .rangehigh = FM_FREQ_RANGE_HIGH,
+ .modulation = V4L2_BAND_MODULATION_FM,
+ },
+ /* Band AM */
+ {
+ .type = V4L2_TUNER_RADIO,
+ .index = 1,
+ .capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+ .rangelow = AM_FREQ_RANGE_LOW,
+ .rangehigh = AM_FREQ_RANGE_HIGH,
+ .modulation = V4L2_BAND_MODULATION_AM,
+ },
+ /* Band SW */
+ {
+ .type = V4L2_TUNER_RADIO,
+ .index = 2,
+ .capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+ .rangelow = SW_FREQ_RANGE_LOW,
+ .rangehigh = SW_FREQ_RANGE_HIGH,
+ .modulation = V4L2_BAND_MODULATION_AM,
+ },
+};
+
+/*
+ * Initialize the RDS generator. If we can loop, then the RDS generator
+ * is set up with the values from the RDS TX controls, otherwise it
+ * will fill in standard values using one of two alternates.
+ */
+void vivid_radio_rds_init(struct vivid_dev *dev)
+{
+ struct vivid_rds_gen *rds = &dev->rds_gen;
+ bool alt = dev->radio_rx_rds_use_alternates;
+
+ /* Do nothing, blocks will be filled by the transmitter */
+ if (dev->radio_rds_loop && !dev->radio_tx_rds_controls)
+ return;
+
+ if (dev->radio_rds_loop) {
+ v4l2_ctrl_lock(dev->radio_tx_rds_pi);
+ rds->picode = dev->radio_tx_rds_pi->cur.val;
+ rds->pty = dev->radio_tx_rds_pty->cur.val;
+ rds->mono_stereo = dev->radio_tx_rds_mono_stereo->cur.val;
+ rds->art_head = dev->radio_tx_rds_art_head->cur.val;
+ rds->compressed = dev->radio_tx_rds_compressed->cur.val;
+ rds->dyn_pty = dev->radio_tx_rds_dyn_pty->cur.val;
+ rds->ta = dev->radio_tx_rds_ta->cur.val;
+ rds->tp = dev->radio_tx_rds_tp->cur.val;
+ rds->ms = dev->radio_tx_rds_ms->cur.val;
+ strscpy(rds->psname,
+ dev->radio_tx_rds_psname->p_cur.p_char,
+ sizeof(rds->psname));
+ strscpy(rds->radiotext,
+ dev->radio_tx_rds_radiotext->p_cur.p_char + alt * 64,
+ sizeof(rds->radiotext));
+ v4l2_ctrl_unlock(dev->radio_tx_rds_pi);
+ } else {
+ vivid_rds_gen_fill(rds, dev->radio_rx_freq, alt);
+ }
+ if (dev->radio_rx_rds_controls) {
+ v4l2_ctrl_s_ctrl(dev->radio_rx_rds_pty, rds->pty);
+ v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ta, rds->ta);
+ v4l2_ctrl_s_ctrl(dev->radio_rx_rds_tp, rds->tp);
+ v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ms, rds->ms);
+ v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_psname, rds->psname);
+ v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_radiotext, rds->radiotext);
+ if (!dev->radio_rds_loop)
+ dev->radio_rx_rds_use_alternates = !dev->radio_rx_rds_use_alternates;
+ }
+ vivid_rds_generate(rds);
+}
+
+/*
+ * Calculate the emulated signal quality taking into account the frequency
+ * the transmitter is using.
+ */
+static void vivid_radio_calc_sig_qual(struct vivid_dev *dev)
+{
+ int mod = 16000;
+ int delta = 800;
+ int sig_qual, sig_qual_tx = mod;
+
+ /*
+ * For SW and FM there is a channel every 1000 kHz, for AM there is one
+ * every 100 kHz.
+ */
+ if (dev->radio_rx_freq <= AM_FREQ_RANGE_HIGH) {
+ mod /= 10;
+ delta /= 10;
+ }
+ sig_qual = (dev->radio_rx_freq + delta) % mod - delta;
+ if (dev->has_radio_tx)
+ sig_qual_tx = dev->radio_rx_freq - dev->radio_tx_freq;
+ if (abs(sig_qual_tx) <= abs(sig_qual)) {
+ sig_qual = sig_qual_tx;
+ /*
+ * Zero the internal rds buffer if we are going to loop
+ * rds blocks.
+ */
+ if (!dev->radio_rds_loop && !dev->radio_tx_rds_controls)
+ memset(dev->rds_gen.data, 0,
+ sizeof(dev->rds_gen.data));
+ dev->radio_rds_loop = dev->radio_rx_freq >= FM_FREQ_RANGE_LOW;
+ } else {
+ dev->radio_rds_loop = false;
+ }
+ if (dev->radio_rx_freq <= AM_FREQ_RANGE_HIGH)
+ sig_qual *= 10;
+ dev->radio_rx_sig_qual = sig_qual;
+}
+
+int vivid_radio_g_frequency(struct file *file, const unsigned *pfreq, struct v4l2_frequency *vf)
+{
+ if (vf->tuner != 0)
+ return -EINVAL;
+ vf->frequency = *pfreq;
+ return 0;
+}
+
+int vivid_radio_s_frequency(struct file *file, unsigned *pfreq, const struct v4l2_frequency *vf)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ unsigned freq;
+ unsigned band;
+
+ if (vf->tuner != 0)
+ return -EINVAL;
+
+ if (vf->frequency >= (FM_FREQ_RANGE_LOW + SW_FREQ_RANGE_HIGH) / 2)
+ band = BAND_FM;
+ else if (vf->frequency <= (AM_FREQ_RANGE_HIGH + SW_FREQ_RANGE_LOW) / 2)
+ band = BAND_AM;
+ else
+ band = BAND_SW;
+
+ freq = clamp_t(u32, vf->frequency, vivid_radio_bands[band].rangelow,
+ vivid_radio_bands[band].rangehigh);
+ *pfreq = freq;
+
+ /*
+ * For both receiver and transmitter recalculate the signal quality
+ * (since that depends on both frequencies) and re-init the rds
+ * generator.
+ */
+ vivid_radio_calc_sig_qual(dev);
+ vivid_radio_rds_init(dev);
+ return 0;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-radio-common.h b/drivers/media/test-drivers/vivid/vivid-radio-common.h
new file mode 100644
index 0000000..30a9900
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-radio-common.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-radio-common.h - common radio rx/tx support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_RADIO_COMMON_H_
+#define _VIVID_RADIO_COMMON_H_
+
+/* The supported radio frequency ranges in kHz */
+#define FM_FREQ_RANGE_LOW (64000U * 16U)
+#define FM_FREQ_RANGE_HIGH (108000U * 16U)
+#define AM_FREQ_RANGE_LOW (520U * 16U)
+#define AM_FREQ_RANGE_HIGH (1710U * 16U)
+#define SW_FREQ_RANGE_LOW (2300U * 16U)
+#define SW_FREQ_RANGE_HIGH (26100U * 16U)
+
+enum { BAND_FM, BAND_AM, BAND_SW, TOT_BANDS };
+
+extern const struct v4l2_frequency_band vivid_radio_bands[TOT_BANDS];
+
+int vivid_radio_g_frequency(struct file *file, const unsigned *freq, struct v4l2_frequency *vf);
+int vivid_radio_s_frequency(struct file *file, unsigned *freq, const struct v4l2_frequency *vf);
+
+void vivid_radio_rds_init(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-radio-rx.c b/drivers/media/test-drivers/vivid/vivid-radio-rx.c
new file mode 100644
index 0000000..232cab5
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-radio-rx.c
@@ -0,0 +1,278 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-radio-rx.c - radio receiver support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <linux/sched/signal.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-radio-common.h"
+#include "vivid-rds-gen.h"
+#include "vivid-radio-rx.h"
+
+ssize_t vivid_radio_rx_read(struct file *file, char __user *buf,
+ size_t size, loff_t *offset)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_rds_data *data = dev->rds_gen.data;
+ bool use_alternates;
+ ktime_t timestamp;
+ unsigned blk;
+ int perc;
+ int i;
+
+ if (dev->radio_rx_rds_controls)
+ return -EINVAL;
+ if (size < sizeof(*data))
+ return 0;
+ size = sizeof(*data) * (size / sizeof(*data));
+
+ if (mutex_lock_interruptible(&dev->mutex))
+ return -ERESTARTSYS;
+ if (dev->radio_rx_rds_owner &&
+ file->private_data != dev->radio_rx_rds_owner) {
+ mutex_unlock(&dev->mutex);
+ return -EBUSY;
+ }
+ if (dev->radio_rx_rds_owner == NULL) {
+ vivid_radio_rds_init(dev);
+ dev->radio_rx_rds_owner = file->private_data;
+ }
+
+retry:
+ timestamp = ktime_sub(ktime_get(), dev->radio_rds_init_time);
+ blk = ktime_divns(timestamp, VIVID_RDS_NSEC_PER_BLK);
+ use_alternates = (blk % VIVID_RDS_GEN_BLOCKS) & 1;
+
+ if (dev->radio_rx_rds_last_block == 0 ||
+ dev->radio_rx_rds_use_alternates != use_alternates) {
+ dev->radio_rx_rds_use_alternates = use_alternates;
+ /* Re-init the RDS generator */
+ vivid_radio_rds_init(dev);
+ }
+ if (blk >= dev->radio_rx_rds_last_block + VIVID_RDS_GEN_BLOCKS)
+ dev->radio_rx_rds_last_block = blk - VIVID_RDS_GEN_BLOCKS + 1;
+
+ /*
+ * No data is available if there hasn't been time to get new data,
+ * or if the RDS receiver has been disabled, or if we use the data
+ * from the RDS transmitter and that RDS transmitter has been disabled,
+ * or if the signal quality is too weak.
+ */
+ if (blk == dev->radio_rx_rds_last_block || !dev->radio_rx_rds_enabled ||
+ (dev->radio_rds_loop && !(dev->radio_tx_subchans & V4L2_TUNER_SUB_RDS)) ||
+ abs(dev->radio_rx_sig_qual) > 200) {
+ mutex_unlock(&dev->mutex);
+ if (file->f_flags & O_NONBLOCK)
+ return -EWOULDBLOCK;
+ if (msleep_interruptible(20) && signal_pending(current))
+ return -EINTR;
+ if (mutex_lock_interruptible(&dev->mutex))
+ return -ERESTARTSYS;
+ goto retry;
+ }
+
+ /* abs(dev->radio_rx_sig_qual) <= 200, map that to a 0-50% range */
+ perc = abs(dev->radio_rx_sig_qual) / 4;
+
+ for (i = 0; i < size && blk > dev->radio_rx_rds_last_block;
+ dev->radio_rx_rds_last_block++) {
+ unsigned data_blk = dev->radio_rx_rds_last_block % VIVID_RDS_GEN_BLOCKS;
+ struct v4l2_rds_data rds = data[data_blk];
+
+ if (data_blk == 0 && dev->radio_rds_loop)
+ vivid_radio_rds_init(dev);
+ if (perc && prandom_u32_max(100) < perc) {
+ switch (prandom_u32_max(4)) {
+ case 0:
+ rds.block |= V4L2_RDS_BLOCK_CORRECTED;
+ break;
+ case 1:
+ rds.block |= V4L2_RDS_BLOCK_INVALID;
+ break;
+ case 2:
+ rds.block |= V4L2_RDS_BLOCK_ERROR;
+ rds.lsb = prandom_u32_max(256);
+ rds.msb = prandom_u32_max(256);
+ break;
+ case 3: /* Skip block altogether */
+ if (i)
+ continue;
+ /*
+ * Must make sure at least one block is
+ * returned, otherwise the application
+ * might think that end-of-file occurred.
+ */
+ break;
+ }
+ }
+ if (copy_to_user(buf + i, &rds, sizeof(rds))) {
+ i = -EFAULT;
+ break;
+ }
+ i += sizeof(rds);
+ }
+ mutex_unlock(&dev->mutex);
+ return i;
+}
+
+__poll_t vivid_radio_rx_poll(struct file *file, struct poll_table_struct *wait)
+{
+ return EPOLLIN | EPOLLRDNORM | v4l2_ctrl_poll(file, wait);
+}
+
+int vivid_radio_rx_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band)
+{
+ if (band->tuner != 0)
+ return -EINVAL;
+
+ if (band->index >= TOT_BANDS)
+ return -EINVAL;
+
+ *band = vivid_radio_bands[band->index];
+ return 0;
+}
+
+int vivid_radio_rx_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2_hw_freq_seek *a)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ unsigned low, high;
+ unsigned freq;
+ unsigned spacing;
+ unsigned band;
+
+ if (a->tuner)
+ return -EINVAL;
+ if (a->wrap_around && dev->radio_rx_hw_seek_mode == VIVID_HW_SEEK_BOUNDED)
+ return -EINVAL;
+
+ if (!a->wrap_around && dev->radio_rx_hw_seek_mode == VIVID_HW_SEEK_WRAP)
+ return -EINVAL;
+ if (!a->rangelow ^ !a->rangehigh)
+ return -EINVAL;
+
+ if (file->f_flags & O_NONBLOCK)
+ return -EWOULDBLOCK;
+
+ if (a->rangelow) {
+ for (band = 0; band < TOT_BANDS; band++)
+ if (a->rangelow >= vivid_radio_bands[band].rangelow &&
+ a->rangehigh <= vivid_radio_bands[band].rangehigh)
+ break;
+ if (band == TOT_BANDS)
+ return -EINVAL;
+ if (!dev->radio_rx_hw_seek_prog_lim &&
+ (a->rangelow != vivid_radio_bands[band].rangelow ||
+ a->rangehigh != vivid_radio_bands[band].rangehigh))
+ return -EINVAL;
+ low = a->rangelow;
+ high = a->rangehigh;
+ } else {
+ for (band = 0; band < TOT_BANDS; band++)
+ if (dev->radio_rx_freq >= vivid_radio_bands[band].rangelow &&
+ dev->radio_rx_freq <= vivid_radio_bands[band].rangehigh)
+ break;
+ if (band == TOT_BANDS)
+ return -EINVAL;
+ low = vivid_radio_bands[band].rangelow;
+ high = vivid_radio_bands[band].rangehigh;
+ }
+ spacing = band == BAND_AM ? 1600 : 16000;
+ freq = clamp(dev->radio_rx_freq, low, high);
+
+ if (a->seek_upward) {
+ freq = spacing * (freq / spacing) + spacing;
+ if (freq > high) {
+ if (!a->wrap_around)
+ return -ENODATA;
+ freq = spacing * (low / spacing) + spacing;
+ if (freq >= dev->radio_rx_freq)
+ return -ENODATA;
+ }
+ } else {
+ freq = spacing * ((freq + spacing - 1) / spacing) - spacing;
+ if (freq < low) {
+ if (!a->wrap_around)
+ return -ENODATA;
+ freq = spacing * ((high + spacing - 1) / spacing) - spacing;
+ if (freq <= dev->radio_rx_freq)
+ return -ENODATA;
+ }
+ }
+ return 0;
+}
+
+int vivid_radio_rx_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ int delta = 800;
+ int sig_qual;
+
+ if (vt->index > 0)
+ return -EINVAL;
+
+ strscpy(vt->name, "AM/FM/SW Receiver", sizeof(vt->name));
+ vt->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+ V4L2_TUNER_CAP_FREQ_BANDS | V4L2_TUNER_CAP_RDS |
+ (dev->radio_rx_rds_controls ?
+ V4L2_TUNER_CAP_RDS_CONTROLS :
+ V4L2_TUNER_CAP_RDS_BLOCK_IO) |
+ (dev->radio_rx_hw_seek_prog_lim ?
+ V4L2_TUNER_CAP_HWSEEK_PROG_LIM : 0);
+ switch (dev->radio_rx_hw_seek_mode) {
+ case VIVID_HW_SEEK_BOUNDED:
+ vt->capability |= V4L2_TUNER_CAP_HWSEEK_BOUNDED;
+ break;
+ case VIVID_HW_SEEK_WRAP:
+ vt->capability |= V4L2_TUNER_CAP_HWSEEK_WRAP;
+ break;
+ case VIVID_HW_SEEK_BOTH:
+ vt->capability |= V4L2_TUNER_CAP_HWSEEK_WRAP |
+ V4L2_TUNER_CAP_HWSEEK_BOUNDED;
+ break;
+ }
+ vt->rangelow = AM_FREQ_RANGE_LOW;
+ vt->rangehigh = FM_FREQ_RANGE_HIGH;
+ sig_qual = dev->radio_rx_sig_qual;
+ vt->signal = abs(sig_qual) > delta ? 0 :
+ 0xffff - ((unsigned)abs(sig_qual) * 0xffff) / delta;
+ vt->afc = sig_qual > delta ? 0 : sig_qual;
+ if (abs(sig_qual) > delta)
+ vt->rxsubchans = 0;
+ else if (dev->radio_rx_freq < FM_FREQ_RANGE_LOW || vt->signal < 0x8000)
+ vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+ else if (dev->radio_rds_loop && !(dev->radio_tx_subchans & V4L2_TUNER_SUB_STEREO))
+ vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+ else
+ vt->rxsubchans = V4L2_TUNER_SUB_STEREO;
+ if (dev->radio_rx_rds_enabled &&
+ (!dev->radio_rds_loop || (dev->radio_tx_subchans & V4L2_TUNER_SUB_RDS)) &&
+ dev->radio_rx_freq >= FM_FREQ_RANGE_LOW && vt->signal >= 0xc000)
+ vt->rxsubchans |= V4L2_TUNER_SUB_RDS;
+ if (dev->radio_rx_rds_controls)
+ vivid_radio_rds_init(dev);
+ vt->audmode = dev->radio_rx_audmode;
+ return 0;
+}
+
+int vivid_radio_rx_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (vt->index)
+ return -EINVAL;
+ dev->radio_rx_audmode = vt->audmode >= V4L2_TUNER_MODE_STEREO;
+ return 0;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-radio-rx.h b/drivers/media/test-drivers/vivid/vivid-radio-rx.h
new file mode 100644
index 0000000..c9c7849
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-radio-rx.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-radio-rx.h - radio receiver support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_RADIO_RX_H_
+#define _VIVID_RADIO_RX_H_
+
+ssize_t vivid_radio_rx_read(struct file *, char __user *, size_t, loff_t *);
+__poll_t vivid_radio_rx_poll(struct file *file, struct poll_table_struct *wait);
+
+int vivid_radio_rx_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band);
+int vivid_radio_rx_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2_hw_freq_seek *a);
+int vivid_radio_rx_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
+int vivid_radio_rx_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-radio-tx.c b/drivers/media/test-drivers/vivid/vivid-radio-tx.c
new file mode 100644
index 0000000..049d40b
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-radio-tx.c
@@ -0,0 +1,128 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-radio-tx.c - radio transmitter support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched/signal.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-tx.h"
+
+ssize_t vivid_radio_tx_write(struct file *file, const char __user *buf,
+ size_t size, loff_t *offset)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_rds_data *data = dev->rds_gen.data;
+ ktime_t timestamp;
+ unsigned blk;
+ int i;
+
+ if (dev->radio_tx_rds_controls)
+ return -EINVAL;
+
+ if (size < sizeof(*data))
+ return -EINVAL;
+ size = sizeof(*data) * (size / sizeof(*data));
+
+ if (mutex_lock_interruptible(&dev->mutex))
+ return -ERESTARTSYS;
+ if (dev->radio_tx_rds_owner &&
+ file->private_data != dev->radio_tx_rds_owner) {
+ mutex_unlock(&dev->mutex);
+ return -EBUSY;
+ }
+ dev->radio_tx_rds_owner = file->private_data;
+
+retry:
+ timestamp = ktime_sub(ktime_get(), dev->radio_rds_init_time);
+ blk = ktime_divns(timestamp, VIVID_RDS_NSEC_PER_BLK);
+ if (blk - VIVID_RDS_GEN_BLOCKS >= dev->radio_tx_rds_last_block)
+ dev->radio_tx_rds_last_block = blk - VIVID_RDS_GEN_BLOCKS + 1;
+
+ /*
+ * No data is available if there hasn't been time to get new data,
+ * or if the RDS receiver has been disabled, or if we use the data
+ * from the RDS transmitter and that RDS transmitter has been disabled,
+ * or if the signal quality is too weak.
+ */
+ if (blk == dev->radio_tx_rds_last_block ||
+ !(dev->radio_tx_subchans & V4L2_TUNER_SUB_RDS)) {
+ mutex_unlock(&dev->mutex);
+ if (file->f_flags & O_NONBLOCK)
+ return -EWOULDBLOCK;
+ if (msleep_interruptible(20) && signal_pending(current))
+ return -EINTR;
+ if (mutex_lock_interruptible(&dev->mutex))
+ return -ERESTARTSYS;
+ goto retry;
+ }
+
+ for (i = 0; i < size && blk > dev->radio_tx_rds_last_block;
+ dev->radio_tx_rds_last_block++) {
+ unsigned data_blk = dev->radio_tx_rds_last_block % VIVID_RDS_GEN_BLOCKS;
+ struct v4l2_rds_data rds;
+
+ if (copy_from_user(&rds, buf + i, sizeof(rds))) {
+ i = -EFAULT;
+ break;
+ }
+ i += sizeof(rds);
+ if (!dev->radio_rds_loop)
+ continue;
+ if ((rds.block & V4L2_RDS_BLOCK_MSK) == V4L2_RDS_BLOCK_INVALID ||
+ (rds.block & V4L2_RDS_BLOCK_ERROR))
+ continue;
+ rds.block &= V4L2_RDS_BLOCK_MSK;
+ data[data_blk] = rds;
+ }
+ mutex_unlock(&dev->mutex);
+ return i;
+}
+
+__poll_t vivid_radio_tx_poll(struct file *file, struct poll_table_struct *wait)
+{
+ return EPOLLOUT | EPOLLWRNORM | v4l2_ctrl_poll(file, wait);
+}
+
+int vidioc_g_modulator(struct file *file, void *fh, struct v4l2_modulator *a)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (a->index > 0)
+ return -EINVAL;
+
+ strscpy(a->name, "AM/FM/SW Transmitter", sizeof(a->name));
+ a->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+ V4L2_TUNER_CAP_FREQ_BANDS | V4L2_TUNER_CAP_RDS |
+ (dev->radio_tx_rds_controls ?
+ V4L2_TUNER_CAP_RDS_CONTROLS :
+ V4L2_TUNER_CAP_RDS_BLOCK_IO);
+ a->rangelow = AM_FREQ_RANGE_LOW;
+ a->rangehigh = FM_FREQ_RANGE_HIGH;
+ a->txsubchans = dev->radio_tx_subchans;
+ return 0;
+}
+
+int vidioc_s_modulator(struct file *file, void *fh, const struct v4l2_modulator *a)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (a->index)
+ return -EINVAL;
+ if (a->txsubchans & ~0x13)
+ return -EINVAL;
+ dev->radio_tx_subchans = a->txsubchans;
+ return 0;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-radio-tx.h b/drivers/media/test-drivers/vivid/vivid-radio-tx.h
new file mode 100644
index 0000000..c2bf1e7
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-radio-tx.h
@@ -0,0 +1,17 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-radio-tx.h - radio transmitter support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_RADIO_TX_H_
+#define _VIVID_RADIO_TX_H_
+
+ssize_t vivid_radio_tx_write(struct file *, const char __user *, size_t, loff_t *);
+__poll_t vivid_radio_tx_poll(struct file *file, struct poll_table_struct *wait);
+
+int vidioc_g_modulator(struct file *file, void *fh, struct v4l2_modulator *a);
+int vidioc_s_modulator(struct file *file, void *fh, const struct v4l2_modulator *a);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-rds-gen.c b/drivers/media/test-drivers/vivid/vivid-rds-gen.c
new file mode 100644
index 0000000..b5b104e
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-rds-gen.c
@@ -0,0 +1,157 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-rds-gen.c - rds (radio data system) generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/kernel.h>
+#include <linux/ktime.h>
+#include <linux/string.h>
+#include <linux/videodev2.h>
+
+#include "vivid-rds-gen.h"
+
+static u8 vivid_get_di(const struct vivid_rds_gen *rds, unsigned grp)
+{
+ switch (grp) {
+ case 0:
+ return (rds->dyn_pty << 2) | (grp & 3);
+ case 1:
+ return (rds->compressed << 2) | (grp & 3);
+ case 2:
+ return (rds->art_head << 2) | (grp & 3);
+ case 3:
+ return (rds->mono_stereo << 2) | (grp & 3);
+ }
+ return 0;
+}
+
+/*
+ * This RDS generator creates 57 RDS groups (one group == four RDS blocks).
+ * Groups 0-3, 22-25 and 44-47 (spaced 22 groups apart) are filled with a
+ * standard 0B group containing the PI code and PS name.
+ *
+ * Groups 4-19 and 26-41 use group 2A for the radio text.
+ *
+ * Group 56 contains the time (group 4A).
+ *
+ * All remaining groups use a filler group 15B block that just repeats
+ * the PI and PTY codes.
+ */
+void vivid_rds_generate(struct vivid_rds_gen *rds)
+{
+ struct v4l2_rds_data *data = rds->data;
+ unsigned grp;
+ unsigned idx;
+ struct tm tm;
+ unsigned date;
+ unsigned time;
+ int l;
+
+ for (grp = 0; grp < VIVID_RDS_GEN_GROUPS; grp++, data += VIVID_RDS_GEN_BLKS_PER_GRP) {
+ data[0].lsb = rds->picode & 0xff;
+ data[0].msb = rds->picode >> 8;
+ data[0].block = V4L2_RDS_BLOCK_A | (V4L2_RDS_BLOCK_A << 3);
+ data[1].lsb = rds->pty << 5;
+ data[1].msb = (rds->pty >> 3) | (rds->tp << 2);
+ data[1].block = V4L2_RDS_BLOCK_B | (V4L2_RDS_BLOCK_B << 3);
+ data[3].block = V4L2_RDS_BLOCK_D | (V4L2_RDS_BLOCK_D << 3);
+
+ switch (grp) {
+ case 0 ... 3:
+ case 22 ... 25:
+ case 44 ... 47: /* Group 0B */
+ idx = (grp % 22) % 4;
+ data[1].lsb |= (rds->ta << 4) | (rds->ms << 3);
+ data[1].lsb |= vivid_get_di(rds, idx);
+ data[1].msb |= 1 << 3;
+ data[2].lsb = rds->picode & 0xff;
+ data[2].msb = rds->picode >> 8;
+ data[2].block = V4L2_RDS_BLOCK_C_ALT | (V4L2_RDS_BLOCK_C_ALT << 3);
+ data[3].lsb = rds->psname[2 * idx + 1];
+ data[3].msb = rds->psname[2 * idx];
+ break;
+ case 4 ... 19:
+ case 26 ... 41: /* Group 2A */
+ idx = ((grp - 4) % 22) % 16;
+ data[1].lsb |= idx;
+ data[1].msb |= 4 << 3;
+ data[2].msb = rds->radiotext[4 * idx];
+ data[2].lsb = rds->radiotext[4 * idx + 1];
+ data[2].block = V4L2_RDS_BLOCK_C | (V4L2_RDS_BLOCK_C << 3);
+ data[3].msb = rds->radiotext[4 * idx + 2];
+ data[3].lsb = rds->radiotext[4 * idx + 3];
+ break;
+ case 56:
+ /*
+ * Group 4A
+ *
+ * Uses the algorithm from Annex G of the RDS standard
+ * EN 50067:1998 to convert a UTC date to an RDS Modified
+ * Julian Day.
+ */
+ time64_to_tm(ktime_get_real_seconds(), 0, &tm);
+ l = tm.tm_mon <= 1;
+ date = 14956 + tm.tm_mday + ((tm.tm_year - l) * 1461) / 4 +
+ ((tm.tm_mon + 2 + l * 12) * 306001) / 10000;
+ time = (tm.tm_hour << 12) |
+ (tm.tm_min << 6) |
+ (sys_tz.tz_minuteswest >= 0 ? 0x20 : 0) |
+ (abs(sys_tz.tz_minuteswest) / 30);
+ data[1].lsb &= ~3;
+ data[1].lsb |= date >> 15;
+ data[1].msb |= 8 << 3;
+ data[2].lsb = (date << 1) & 0xfe;
+ data[2].lsb |= (time >> 16) & 1;
+ data[2].msb = (date >> 7) & 0xff;
+ data[2].block = V4L2_RDS_BLOCK_C | (V4L2_RDS_BLOCK_C << 3);
+ data[3].lsb = time & 0xff;
+ data[3].msb = (time >> 8) & 0xff;
+ break;
+ default: /* Group 15B */
+ data[1].lsb |= (rds->ta << 4) | (rds->ms << 3);
+ data[1].lsb |= vivid_get_di(rds, grp % 22);
+ data[1].msb |= 0x1f << 3;
+ data[2].lsb = rds->picode & 0xff;
+ data[2].msb = rds->picode >> 8;
+ data[2].block = V4L2_RDS_BLOCK_C_ALT | (V4L2_RDS_BLOCK_C_ALT << 3);
+ data[3].lsb = rds->pty << 5;
+ data[3].lsb |= (rds->ta << 4) | (rds->ms << 3);
+ data[3].lsb |= vivid_get_di(rds, grp % 22);
+ data[3].msb |= rds->pty >> 3;
+ data[3].msb |= 0x1f << 3;
+ break;
+ }
+ }
+}
+
+void vivid_rds_gen_fill(struct vivid_rds_gen *rds, unsigned freq,
+ bool alt)
+{
+ /* Alternate PTY between Info and Weather */
+ if (rds->use_rbds) {
+ rds->picode = 0x2e75; /* 'KLNX' call sign */
+ rds->pty = alt ? 29 : 2;
+ } else {
+ rds->picode = 0x8088;
+ rds->pty = alt ? 16 : 3;
+ }
+ rds->mono_stereo = true;
+ rds->art_head = false;
+ rds->compressed = false;
+ rds->dyn_pty = false;
+ rds->tp = true;
+ rds->ta = alt;
+ rds->ms = true;
+ snprintf(rds->psname, sizeof(rds->psname), "%6d.%1d",
+ freq / 16, ((freq & 0xf) * 10) / 16);
+ if (alt)
+ strscpy(rds->radiotext,
+ " The Radio Data System can switch between different Radio Texts ",
+ sizeof(rds->radiotext));
+ else
+ strscpy(rds->radiotext,
+ "An example of Radio Text as transmitted by the Radio Data System",
+ sizeof(rds->radiotext));
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-rds-gen.h b/drivers/media/test-drivers/vivid/vivid-rds-gen.h
new file mode 100644
index 0000000..35ac574
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-rds-gen.h
@@ -0,0 +1,42 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-rds-gen.h - rds (radio data system) generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_RDS_GEN_H_
+#define _VIVID_RDS_GEN_H_
+
+/*
+ * It takes almost exactly 5 seconds to transmit 57 RDS groups.
+ * Each group has 4 blocks and each block has a payload of 16 bits + a
+ * block identification. The driver will generate the contents of these
+ * 57 groups only when necessary and it will just be played continuously.
+ */
+#define VIVID_RDS_GEN_GROUPS 57
+#define VIVID_RDS_GEN_BLKS_PER_GRP 4
+#define VIVID_RDS_GEN_BLOCKS (VIVID_RDS_GEN_BLKS_PER_GRP * VIVID_RDS_GEN_GROUPS)
+#define VIVID_RDS_NSEC_PER_BLK (u32)(5ull * NSEC_PER_SEC / VIVID_RDS_GEN_BLOCKS)
+
+struct vivid_rds_gen {
+ struct v4l2_rds_data data[VIVID_RDS_GEN_BLOCKS];
+ bool use_rbds;
+ u16 picode;
+ u8 pty;
+ bool mono_stereo;
+ bool art_head;
+ bool compressed;
+ bool dyn_pty;
+ bool ta;
+ bool tp;
+ bool ms;
+ char psname[8 + 1];
+ char radiotext[64 + 1];
+};
+
+void vivid_rds_gen_fill(struct vivid_rds_gen *rds, unsigned freq,
+ bool use_alternate);
+void vivid_rds_generate(struct vivid_rds_gen *rds);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-sdr-cap.c b/drivers/media/test-drivers/vivid/vivid-sdr-cap.c
new file mode 100644
index 0000000..2b7522e
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-sdr-cap.c
@@ -0,0 +1,570 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-sdr-cap.c - software defined radio support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/math64.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+#include <linux/fixp-arith.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-sdr-cap.h"
+
+/* stream formats */
+struct vivid_format {
+ u32 pixelformat;
+ u32 buffersize;
+};
+
+/* format descriptions for capture and preview */
+static const struct vivid_format formats[] = {
+ {
+ .pixelformat = V4L2_SDR_FMT_CU8,
+ .buffersize = SDR_CAP_SAMPLES_PER_BUF * 2,
+ }, {
+ .pixelformat = V4L2_SDR_FMT_CS8,
+ .buffersize = SDR_CAP_SAMPLES_PER_BUF * 2,
+ },
+};
+
+static const struct v4l2_frequency_band bands_adc[] = {
+ {
+ .tuner = 0,
+ .type = V4L2_TUNER_ADC,
+ .index = 0,
+ .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+ .rangelow = 300000,
+ .rangehigh = 300000,
+ },
+ {
+ .tuner = 0,
+ .type = V4L2_TUNER_ADC,
+ .index = 1,
+ .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+ .rangelow = 900001,
+ .rangehigh = 2800000,
+ },
+ {
+ .tuner = 0,
+ .type = V4L2_TUNER_ADC,
+ .index = 2,
+ .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+ .rangelow = 3200000,
+ .rangehigh = 3200000,
+ },
+};
+
+/* ADC band midpoints */
+#define BAND_ADC_0 ((bands_adc[0].rangehigh + bands_adc[1].rangelow) / 2)
+#define BAND_ADC_1 ((bands_adc[1].rangehigh + bands_adc[2].rangelow) / 2)
+
+static const struct v4l2_frequency_band bands_fm[] = {
+ {
+ .tuner = 1,
+ .type = V4L2_TUNER_RF,
+ .index = 0,
+ .capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+ .rangelow = 50000000,
+ .rangehigh = 2000000000,
+ },
+};
+
+static void vivid_thread_sdr_cap_tick(struct vivid_dev *dev)
+{
+ struct vivid_buffer *sdr_cap_buf = NULL;
+
+ dprintk(dev, 1, "SDR Capture Thread Tick\n");
+
+ /* Drop a certain percentage of buffers. */
+ if (dev->perc_dropped_buffers &&
+ prandom_u32_max(100) < dev->perc_dropped_buffers)
+ return;
+
+ spin_lock(&dev->slock);
+ if (!list_empty(&dev->sdr_cap_active)) {
+ sdr_cap_buf = list_entry(dev->sdr_cap_active.next,
+ struct vivid_buffer, list);
+ list_del(&sdr_cap_buf->list);
+ }
+ spin_unlock(&dev->slock);
+
+ if (sdr_cap_buf) {
+ sdr_cap_buf->vb.sequence = dev->sdr_cap_seq_count;
+ v4l2_ctrl_request_setup(sdr_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_sdr_cap);
+ v4l2_ctrl_request_complete(sdr_cap_buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_sdr_cap);
+ vivid_sdr_cap_process(dev, sdr_cap_buf);
+ sdr_cap_buf->vb.vb2_buf.timestamp =
+ ktime_get_ns() + dev->time_wrap_offset;
+ vb2_buffer_done(&sdr_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
+ VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+ dev->dqbuf_error = false;
+ }
+}
+
+static int vivid_thread_sdr_cap(void *data)
+{
+ struct vivid_dev *dev = data;
+ u64 samples_since_start;
+ u64 buffers_since_start;
+ u64 next_jiffies_since_start;
+ unsigned long jiffies_since_start;
+ unsigned long cur_jiffies;
+ unsigned wait_jiffies;
+
+ dprintk(dev, 1, "SDR Capture Thread Start\n");
+
+ set_freezable();
+
+ /* Resets frame counters */
+ dev->sdr_cap_seq_offset = 0;
+ if (dev->seq_wrap)
+ dev->sdr_cap_seq_offset = 0xffffff80U;
+ dev->jiffies_sdr_cap = jiffies;
+ dev->sdr_cap_seq_resync = false;
+
+ for (;;) {
+ try_to_freeze();
+ if (kthread_should_stop())
+ break;
+
+ if (!mutex_trylock(&dev->mutex)) {
+ schedule_timeout_uninterruptible(1);
+ continue;
+ }
+
+ cur_jiffies = jiffies;
+ if (dev->sdr_cap_seq_resync) {
+ dev->jiffies_sdr_cap = cur_jiffies;
+ dev->sdr_cap_seq_offset = dev->sdr_cap_seq_count + 1;
+ dev->sdr_cap_seq_count = 0;
+ dev->sdr_cap_seq_resync = false;
+ }
+ /* Calculate the number of jiffies since we started streaming */
+ jiffies_since_start = cur_jiffies - dev->jiffies_sdr_cap;
+ /* Get the number of buffers streamed since the start */
+ buffers_since_start =
+ (u64)jiffies_since_start * dev->sdr_adc_freq +
+ (HZ * SDR_CAP_SAMPLES_PER_BUF) / 2;
+ do_div(buffers_since_start, HZ * SDR_CAP_SAMPLES_PER_BUF);
+
+ /*
+ * After more than 0xf0000000 (rounded down to a multiple of
+ * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+ * jiffies have passed since we started streaming reset the
+ * counters and keep track of the sequence offset.
+ */
+ if (jiffies_since_start > JIFFIES_RESYNC) {
+ dev->jiffies_sdr_cap = cur_jiffies;
+ dev->sdr_cap_seq_offset = buffers_since_start;
+ buffers_since_start = 0;
+ }
+ dev->sdr_cap_seq_count =
+ buffers_since_start + dev->sdr_cap_seq_offset;
+
+ vivid_thread_sdr_cap_tick(dev);
+ mutex_unlock(&dev->mutex);
+
+ /*
+ * Calculate the number of samples streamed since we started,
+ * not including the current buffer.
+ */
+ samples_since_start = buffers_since_start * SDR_CAP_SAMPLES_PER_BUF;
+
+ /* And the number of jiffies since we started */
+ jiffies_since_start = jiffies - dev->jiffies_sdr_cap;
+
+ /* Increase by the number of samples in one buffer */
+ samples_since_start += SDR_CAP_SAMPLES_PER_BUF;
+ /*
+ * Calculate when that next buffer is supposed to start
+ * in jiffies since we started streaming.
+ */
+ next_jiffies_since_start = samples_since_start * HZ +
+ dev->sdr_adc_freq / 2;
+ do_div(next_jiffies_since_start, dev->sdr_adc_freq);
+ /* If it is in the past, then just schedule asap */
+ if (next_jiffies_since_start < jiffies_since_start)
+ next_jiffies_since_start = jiffies_since_start;
+
+ wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+ schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+ }
+ dprintk(dev, 1, "SDR Capture Thread End\n");
+ return 0;
+}
+
+static int sdr_cap_queue_setup(struct vb2_queue *vq,
+ unsigned *nbuffers, unsigned *nplanes,
+ unsigned sizes[], struct device *alloc_devs[])
+{
+ /* 2 = max 16-bit sample returned */
+ sizes[0] = SDR_CAP_SAMPLES_PER_BUF * 2;
+ *nplanes = 1;
+ return 0;
+}
+
+static int sdr_cap_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ unsigned size = SDR_CAP_SAMPLES_PER_BUF * 2;
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->buf_prepare_error) {
+ /*
+ * Error injection: test what happens if buf_prepare() returns
+ * an error.
+ */
+ dev->buf_prepare_error = false;
+ return -EINVAL;
+ }
+ if (vb2_plane_size(vb, 0) < size) {
+ dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+ __func__, vb2_plane_size(vb, 0), size);
+ return -EINVAL;
+ }
+ vb2_set_plane_payload(vb, 0, size);
+
+ return 0;
+}
+
+static void sdr_cap_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ spin_lock(&dev->slock);
+ list_add_tail(&buf->list, &dev->sdr_cap_active);
+ spin_unlock(&dev->slock);
+}
+
+static int sdr_cap_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ int err = 0;
+
+ dprintk(dev, 1, "%s\n", __func__);
+ dev->sdr_cap_seq_count = 0;
+ if (dev->start_streaming_error) {
+ dev->start_streaming_error = false;
+ err = -EINVAL;
+ } else if (dev->kthread_sdr_cap == NULL) {
+ dev->kthread_sdr_cap = kthread_run(vivid_thread_sdr_cap, dev,
+ "%s-sdr-cap", dev->v4l2_dev.name);
+
+ if (IS_ERR(dev->kthread_sdr_cap)) {
+ v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+ err = PTR_ERR(dev->kthread_sdr_cap);
+ dev->kthread_sdr_cap = NULL;
+ }
+ }
+ if (err) {
+ struct vivid_buffer *buf, *tmp;
+
+ list_for_each_entry_safe(buf, tmp, &dev->sdr_cap_active, list) {
+ list_del(&buf->list);
+ vb2_buffer_done(&buf->vb.vb2_buf,
+ VB2_BUF_STATE_QUEUED);
+ }
+ }
+ return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void sdr_cap_stop_streaming(struct vb2_queue *vq)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+ if (dev->kthread_sdr_cap == NULL)
+ return;
+
+ while (!list_empty(&dev->sdr_cap_active)) {
+ struct vivid_buffer *buf;
+
+ buf = list_entry(dev->sdr_cap_active.next,
+ struct vivid_buffer, list);
+ list_del(&buf->list);
+ v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req,
+ &dev->ctrl_hdl_sdr_cap);
+ vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
+ }
+
+ /* shutdown control thread */
+ kthread_stop(dev->kthread_sdr_cap);
+ dev->kthread_sdr_cap = NULL;
+}
+
+static void sdr_cap_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_sdr_cap);
+}
+
+const struct vb2_ops vivid_sdr_cap_qops = {
+ .queue_setup = sdr_cap_queue_setup,
+ .buf_prepare = sdr_cap_buf_prepare,
+ .buf_queue = sdr_cap_buf_queue,
+ .start_streaming = sdr_cap_start_streaming,
+ .stop_streaming = sdr_cap_stop_streaming,
+ .buf_request_complete = sdr_cap_buf_request_complete,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+int vivid_sdr_enum_freq_bands(struct file *file, void *fh,
+ struct v4l2_frequency_band *band)
+{
+ switch (band->tuner) {
+ case 0:
+ if (band->index >= ARRAY_SIZE(bands_adc))
+ return -EINVAL;
+ *band = bands_adc[band->index];
+ return 0;
+ case 1:
+ if (band->index >= ARRAY_SIZE(bands_fm))
+ return -EINVAL;
+ *band = bands_fm[band->index];
+ return 0;
+ default:
+ return -EINVAL;
+ }
+}
+
+int vivid_sdr_g_frequency(struct file *file, void *fh,
+ struct v4l2_frequency *vf)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ switch (vf->tuner) {
+ case 0:
+ vf->frequency = dev->sdr_adc_freq;
+ vf->type = V4L2_TUNER_ADC;
+ return 0;
+ case 1:
+ vf->frequency = dev->sdr_fm_freq;
+ vf->type = V4L2_TUNER_RF;
+ return 0;
+ default:
+ return -EINVAL;
+ }
+}
+
+int vivid_sdr_s_frequency(struct file *file, void *fh,
+ const struct v4l2_frequency *vf)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ unsigned freq = vf->frequency;
+ unsigned band;
+
+ switch (vf->tuner) {
+ case 0:
+ if (vf->type != V4L2_TUNER_ADC)
+ return -EINVAL;
+ if (freq < BAND_ADC_0)
+ band = 0;
+ else if (freq < BAND_ADC_1)
+ band = 1;
+ else
+ band = 2;
+
+ freq = clamp_t(unsigned, freq,
+ bands_adc[band].rangelow,
+ bands_adc[band].rangehigh);
+
+ if (vb2_is_streaming(&dev->vb_sdr_cap_q) &&
+ freq != dev->sdr_adc_freq) {
+ /* resync the thread's timings */
+ dev->sdr_cap_seq_resync = true;
+ }
+ dev->sdr_adc_freq = freq;
+ return 0;
+ case 1:
+ if (vf->type != V4L2_TUNER_RF)
+ return -EINVAL;
+ dev->sdr_fm_freq = clamp_t(unsigned, freq,
+ bands_fm[0].rangelow,
+ bands_fm[0].rangehigh);
+ return 0;
+ default:
+ return -EINVAL;
+ }
+}
+
+int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+ switch (vt->index) {
+ case 0:
+ strscpy(vt->name, "ADC", sizeof(vt->name));
+ vt->type = V4L2_TUNER_ADC;
+ vt->capability =
+ V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+ vt->rangelow = bands_adc[0].rangelow;
+ vt->rangehigh = bands_adc[2].rangehigh;
+ return 0;
+ case 1:
+ strscpy(vt->name, "RF", sizeof(vt->name));
+ vt->type = V4L2_TUNER_RF;
+ vt->capability =
+ V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+ vt->rangelow = bands_fm[0].rangelow;
+ vt->rangehigh = bands_fm[0].rangehigh;
+ return 0;
+ default:
+ return -EINVAL;
+ }
+}
+
+int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+ if (vt->index > 1)
+ return -EINVAL;
+ return 0;
+}
+
+int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+ if (f->index >= ARRAY_SIZE(formats))
+ return -EINVAL;
+ f->pixelformat = formats[f->index].pixelformat;
+ return 0;
+}
+
+int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ f->fmt.sdr.pixelformat = dev->sdr_pixelformat;
+ f->fmt.sdr.buffersize = dev->sdr_buffersize;
+ memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+ return 0;
+}
+
+int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct vb2_queue *q = &dev->vb_sdr_cap_q;
+ int i;
+
+ if (vb2_is_busy(q))
+ return -EBUSY;
+
+ memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+ for (i = 0; i < ARRAY_SIZE(formats); i++) {
+ if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
+ dev->sdr_pixelformat = formats[i].pixelformat;
+ dev->sdr_buffersize = formats[i].buffersize;
+ f->fmt.sdr.buffersize = formats[i].buffersize;
+ return 0;
+ }
+ }
+ dev->sdr_pixelformat = formats[0].pixelformat;
+ dev->sdr_buffersize = formats[0].buffersize;
+ f->fmt.sdr.pixelformat = formats[0].pixelformat;
+ f->fmt.sdr.buffersize = formats[0].buffersize;
+ return 0;
+}
+
+int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+ int i;
+
+ memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+ for (i = 0; i < ARRAY_SIZE(formats); i++) {
+ if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
+ f->fmt.sdr.buffersize = formats[i].buffersize;
+ return 0;
+ }
+ }
+ f->fmt.sdr.pixelformat = formats[0].pixelformat;
+ f->fmt.sdr.buffersize = formats[0].buffersize;
+ return 0;
+}
+
+#define FIXP_N (15)
+#define FIXP_FRAC (1 << FIXP_N)
+#define FIXP_2PI ((int)(2 * 3.141592653589 * FIXP_FRAC))
+#define M_100000PI (3.14159 * 100000)
+
+void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+ u8 *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+ unsigned long i;
+ unsigned long plane_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
+ s64 s64tmp;
+ s32 src_phase_step;
+ s32 mod_phase_step;
+ s32 fixp_i;
+ s32 fixp_q;
+
+ /* calculate phase step */
+ #define BEEP_FREQ 1000 /* 1kHz beep */
+ src_phase_step = DIV_ROUND_CLOSEST(FIXP_2PI * BEEP_FREQ,
+ dev->sdr_adc_freq);
+
+ for (i = 0; i < plane_size; i += 2) {
+ mod_phase_step = fixp_cos32_rad(dev->sdr_fixp_src_phase,
+ FIXP_2PI) >> (31 - FIXP_N);
+
+ dev->sdr_fixp_src_phase += src_phase_step;
+ s64tmp = (s64) mod_phase_step * dev->sdr_fm_deviation;
+ dev->sdr_fixp_mod_phase += div_s64(s64tmp, M_100000PI);
+
+ /*
+ * Transfer phase angle to [0, 2xPI] in order to avoid variable
+ * overflow and make it suitable for cosine implementation
+ * used, which does not support negative angles.
+ */
+ dev->sdr_fixp_src_phase %= FIXP_2PI;
+ dev->sdr_fixp_mod_phase %= FIXP_2PI;
+
+ if (dev->sdr_fixp_mod_phase < 0)
+ dev->sdr_fixp_mod_phase += FIXP_2PI;
+
+ fixp_i = fixp_cos32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI);
+ fixp_q = fixp_sin32_rad(dev->sdr_fixp_mod_phase, FIXP_2PI);
+
+ /* Normalize fraction values represented with 32 bit precision
+ * to fixed point representation with FIXP_N bits */
+ fixp_i >>= (31 - FIXP_N);
+ fixp_q >>= (31 - FIXP_N);
+
+ switch (dev->sdr_pixelformat) {
+ case V4L2_SDR_FMT_CU8:
+ /* convert 'fixp float' to u8 [0, +255] */
+ /* u8 = X * 127.5 + 127.5; X is float [-1.0, +1.0] */
+ fixp_i = fixp_i * 1275 + FIXP_FRAC * 1275;
+ fixp_q = fixp_q * 1275 + FIXP_FRAC * 1275;
+ *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10);
+ *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10);
+ break;
+ case V4L2_SDR_FMT_CS8:
+ /* convert 'fixp float' to s8 [-128, +127] */
+ /* s8 = X * 127.5 - 0.5; X is float [-1.0, +1.0] */
+ fixp_i = fixp_i * 1275 - FIXP_FRAC * 5;
+ fixp_q = fixp_q * 1275 - FIXP_FRAC * 5;
+ *vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10);
+ *vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10);
+ break;
+ default:
+ break;
+ }
+ }
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-sdr-cap.h b/drivers/media/test-drivers/vivid/vivid-sdr-cap.h
new file mode 100644
index 0000000..813c924
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-sdr-cap.h
@@ -0,0 +1,24 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-sdr-cap.h - software defined radio support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_SDR_CAP_H_
+#define _VIVID_SDR_CAP_H_
+
+int vivid_sdr_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band);
+int vivid_sdr_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+int vivid_sdr_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
+int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
+int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
+int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
+int vidioc_s_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
+int vidioc_try_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
+void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+
+extern const struct vb2_ops vivid_sdr_cap_qops;
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-touch-cap.c b/drivers/media/test-drivers/vivid/vivid-touch-cap.c
new file mode 100644
index 0000000..ebb00b1
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-touch-cap.c
@@ -0,0 +1,341 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-touch-cap.c - touch support functions.
+ */
+
+#include "vivid-core.h"
+#include "vivid-kthread-touch.h"
+#include "vivid-vid-common.h"
+#include "vivid-touch-cap.h"
+
+static int touch_cap_queue_setup(struct vb2_queue *vq, unsigned int *nbuffers,
+ unsigned int *nplanes, unsigned int sizes[],
+ struct device *alloc_devs[])
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ struct v4l2_pix_format *f = &dev->tch_format;
+ unsigned int size = f->sizeimage;
+
+ if (*nplanes) {
+ if (sizes[0] < size)
+ return -EINVAL;
+ } else {
+ sizes[0] = size;
+ }
+
+ if (vq->num_buffers + *nbuffers < 2)
+ *nbuffers = 2 - vq->num_buffers;
+
+ *nplanes = 1;
+ return 0;
+}
+
+static int touch_cap_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct v4l2_pix_format *f = &dev->tch_format;
+ unsigned int size = f->sizeimage;
+
+ if (dev->buf_prepare_error) {
+ /*
+ * Error injection: test what happens if buf_prepare() returns
+ * an error.
+ */
+ dev->buf_prepare_error = false;
+ return -EINVAL;
+ }
+ if (vb2_plane_size(vb, 0) < size) {
+ dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+ __func__, vb2_plane_size(vb, 0), size);
+ return -EINVAL;
+ }
+ vb2_set_plane_payload(vb, 0, size);
+
+ return 0;
+}
+
+static void touch_cap_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+ vbuf->field = V4L2_FIELD_NONE;
+ spin_lock(&dev->slock);
+ list_add_tail(&buf->list, &dev->touch_cap_active);
+ spin_unlock(&dev->slock);
+}
+
+static int touch_cap_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ int err;
+
+ dev->touch_cap_seq_count = 0;
+ if (dev->start_streaming_error) {
+ dev->start_streaming_error = false;
+ err = -EINVAL;
+ } else {
+ err = vivid_start_generating_touch_cap(dev);
+ }
+ if (err) {
+ struct vivid_buffer *buf, *tmp;
+
+ list_for_each_entry_safe(buf, tmp,
+ &dev->touch_cap_active, list) {
+ list_del(&buf->list);
+ vb2_buffer_done(&buf->vb.vb2_buf,
+ VB2_BUF_STATE_QUEUED);
+ }
+ }
+ return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void touch_cap_stop_streaming(struct vb2_queue *vq)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+ vivid_stop_generating_touch_cap(dev);
+}
+
+static void touch_cap_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_touch_cap);
+}
+
+const struct vb2_ops vivid_touch_cap_qops = {
+ .queue_setup = touch_cap_queue_setup,
+ .buf_prepare = touch_cap_buf_prepare,
+ .buf_queue = touch_cap_buf_queue,
+ .start_streaming = touch_cap_start_streaming,
+ .stop_streaming = touch_cap_stop_streaming,
+ .buf_request_complete = touch_cap_buf_request_complete,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+int vivid_enum_fmt_tch(struct file *file, void *priv, struct v4l2_fmtdesc *f)
+{
+ if (f->index)
+ return -EINVAL;
+
+ f->pixelformat = V4L2_TCH_FMT_DELTA_TD16;
+ return 0;
+}
+
+int vivid_g_fmt_tch(struct file *file, void *priv, struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+ f->fmt.pix = dev->tch_format;
+ return 0;
+}
+
+int vivid_g_fmt_tch_mplane(struct file *file, void *priv, struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_format sp_fmt;
+
+ if (!dev->multiplanar)
+ return -ENOTTY;
+ sp_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ sp_fmt.fmt.pix = dev->tch_format;
+ fmt_sp2mp(&sp_fmt, f);
+ return 0;
+}
+
+int vivid_g_parm_tch(struct file *file, void *priv,
+ struct v4l2_streamparm *parm)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (parm->type != (dev->multiplanar ?
+ V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+ V4L2_BUF_TYPE_VIDEO_CAPTURE))
+ return -EINVAL;
+
+ parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+ parm->parm.capture.timeperframe = dev->timeperframe_tch_cap;
+ parm->parm.capture.readbuffers = 1;
+ return 0;
+}
+
+int vivid_enum_input_tch(struct file *file, void *priv, struct v4l2_input *inp)
+{
+ if (inp->index)
+ return -EINVAL;
+
+ inp->type = V4L2_INPUT_TYPE_TOUCH;
+ strscpy(inp->name, "Vivid Touch", sizeof(inp->name));
+ inp->capabilities = 0;
+ return 0;
+}
+
+int vivid_g_input_tch(struct file *file, void *priv, unsigned int *i)
+{
+ *i = 0;
+ return 0;
+}
+
+int vivid_set_touch(struct vivid_dev *dev, unsigned int i)
+{
+ struct v4l2_pix_format *f = &dev->tch_format;
+
+ if (i)
+ return -EINVAL;
+
+ f->pixelformat = V4L2_TCH_FMT_DELTA_TD16;
+ f->width = VIVID_TCH_WIDTH;
+ f->height = VIVID_TCH_HEIGHT;
+ f->field = V4L2_FIELD_NONE;
+ f->colorspace = V4L2_COLORSPACE_RAW;
+ f->bytesperline = f->width * sizeof(s16);
+ f->sizeimage = f->width * f->height * sizeof(s16);
+ return 0;
+}
+
+int vivid_s_input_tch(struct file *file, void *priv, unsigned int i)
+{
+ return vivid_set_touch(video_drvdata(file), i);
+}
+
+static void vivid_fill_buff_noise(__s16 *tch_buf, int size)
+{
+ int i;
+
+ /* Fill 10% of the values within range -3 and 3, zero the others */
+ for (i = 0; i < size; i++) {
+ unsigned int rand = get_random_int();
+
+ if (rand % 10)
+ tch_buf[i] = 0;
+ else
+ tch_buf[i] = (rand / 10) % 7 - 3;
+ }
+}
+
+static inline int get_random_pressure(void)
+{
+ return get_random_int() % VIVID_PRESSURE_LIMIT;
+}
+
+static void vivid_tch_buf_set(struct v4l2_pix_format *f,
+ __s16 *tch_buf,
+ int index)
+{
+ unsigned int x = index % f->width;
+ unsigned int y = index / f->width;
+ unsigned int offset = VIVID_MIN_PRESSURE;
+
+ tch_buf[index] = offset + get_random_pressure();
+ offset /= 2;
+ if (x)
+ tch_buf[index - 1] = offset + get_random_pressure();
+ if (x < f->width - 1)
+ tch_buf[index + 1] = offset + get_random_pressure();
+ if (y)
+ tch_buf[index - f->width] = offset + get_random_pressure();
+ if (y < f->height - 1)
+ tch_buf[index + f->width] = offset + get_random_pressure();
+ offset /= 2;
+ if (x && y)
+ tch_buf[index - 1 - f->width] = offset + get_random_pressure();
+ if (x < f->width - 1 && y)
+ tch_buf[index + 1 - f->width] = offset + get_random_pressure();
+ if (x && y < f->height - 1)
+ tch_buf[index - 1 + f->width] = offset + get_random_pressure();
+ if (x < f->width - 1 && y < f->height - 1)
+ tch_buf[index + 1 + f->width] = offset + get_random_pressure();
+}
+
+void vivid_fillbuff_tch(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+ struct v4l2_pix_format *f = &dev->tch_format;
+ int size = f->width * f->height;
+ int x, y, xstart, ystart, offset_x, offset_y;
+ unsigned int test_pattern, test_pat_idx, rand;
+
+ __s16 *tch_buf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+
+ buf->vb.sequence = dev->touch_cap_seq_count;
+ test_pattern = (buf->vb.sequence / TCH_SEQ_COUNT) % TEST_CASE_MAX;
+ test_pat_idx = buf->vb.sequence % TCH_SEQ_COUNT;
+
+ vivid_fill_buff_noise(tch_buf, size);
+
+ if (test_pat_idx >= TCH_PATTERN_COUNT)
+ return;
+
+ if (test_pat_idx == 0)
+ dev->tch_pat_random = get_random_int();
+ rand = dev->tch_pat_random;
+
+ switch (test_pattern) {
+ case SINGLE_TAP:
+ if (test_pat_idx == 2)
+ vivid_tch_buf_set(f, tch_buf, rand % size);
+ break;
+ case DOUBLE_TAP:
+ if (test_pat_idx == 2 || test_pat_idx == 4)
+ vivid_tch_buf_set(f, tch_buf, rand % size);
+ break;
+ case TRIPLE_TAP:
+ if (test_pat_idx == 2 || test_pat_idx == 4 || test_pat_idx == 6)
+ vivid_tch_buf_set(f, tch_buf, rand % size);
+ break;
+ case MOVE_LEFT_TO_RIGHT:
+ vivid_tch_buf_set(f, tch_buf,
+ (rand % f->height) * f->width +
+ test_pat_idx *
+ (f->width / TCH_PATTERN_COUNT));
+ break;
+ case ZOOM_IN:
+ x = f->width / 2;
+ y = f->height / 2;
+ offset_x = ((TCH_PATTERN_COUNT - 1 - test_pat_idx) * x) /
+ TCH_PATTERN_COUNT;
+ offset_y = ((TCH_PATTERN_COUNT - 1 - test_pat_idx) * y) /
+ TCH_PATTERN_COUNT;
+ vivid_tch_buf_set(f, tch_buf,
+ (x - offset_x) + f->width * (y - offset_y));
+ vivid_tch_buf_set(f, tch_buf,
+ (x + offset_x) + f->width * (y + offset_y));
+ break;
+ case ZOOM_OUT:
+ x = f->width / 2;
+ y = f->height / 2;
+ offset_x = (test_pat_idx * x) / TCH_PATTERN_COUNT;
+ offset_y = (test_pat_idx * y) / TCH_PATTERN_COUNT;
+ vivid_tch_buf_set(f, tch_buf,
+ (x - offset_x) + f->width * (y - offset_y));
+ vivid_tch_buf_set(f, tch_buf,
+ (x + offset_x) + f->width * (y + offset_y));
+ break;
+ case PALM_PRESS:
+ for (x = 0; x < f->width; x++)
+ for (y = f->height / 2; y < f->height; y++)
+ tch_buf[x + f->width * y] = VIVID_MIN_PRESSURE +
+ get_random_pressure();
+ break;
+ case MULTIPLE_PRESS:
+ /* 16 pressure points */
+ for (y = 0; y < 4; y++) {
+ for (x = 0; x < 4; x++) {
+ ystart = (y * f->height) / 4 + f->height / 8;
+ xstart = (x * f->width) / 4 + f->width / 8;
+ vivid_tch_buf_set(f, tch_buf,
+ ystart * f->width + xstart);
+ }
+ }
+ break;
+ }
+#ifdef __BIG_ENDIAN__
+ for (x = 0; x < size; x++)
+ tch_buf[x] = (__force s16)__cpu_to_le16((u16)tch_buf[x]);
+#endif
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-touch-cap.h b/drivers/media/test-drivers/vivid/vivid-touch-cap.h
new file mode 100644
index 0000000..07e5140
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-touch-cap.h
@@ -0,0 +1,39 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-touch-cap.h - touch support functions.
+ */
+#ifndef _VIVID_TOUCH_CAP_H_
+#define _VIVID_TOUCH_CAP_H_
+
+#define VIVID_TCH_HEIGHT 12
+#define VIVID_TCH_WIDTH 21
+#define VIVID_MIN_PRESSURE 180
+#define VIVID_PRESSURE_LIMIT 40
+#define TCH_SEQ_COUNT 16
+#define TCH_PATTERN_COUNT 12
+
+enum vivid_tch_test {
+ SINGLE_TAP,
+ DOUBLE_TAP,
+ TRIPLE_TAP,
+ MOVE_LEFT_TO_RIGHT,
+ ZOOM_IN,
+ ZOOM_OUT,
+ PALM_PRESS,
+ MULTIPLE_PRESS,
+ TEST_CASE_MAX
+};
+
+extern const struct vb2_ops vivid_touch_cap_qops;
+
+int vivid_enum_fmt_tch(struct file *file, void *priv, struct v4l2_fmtdesc *f);
+int vivid_g_fmt_tch(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_g_fmt_tch_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_enum_input_tch(struct file *file, void *priv, struct v4l2_input *inp);
+int vivid_g_input_tch(struct file *file, void *priv, unsigned int *i);
+int vivid_s_input_tch(struct file *file, void *priv, unsigned int i);
+void vivid_fillbuff_tch(struct vivid_dev *dev, struct vivid_buffer *buf);
+int vivid_set_touch(struct vivid_dev *dev, unsigned int i);
+int vivid_g_parm_tch(struct file *file, void *priv,
+ struct v4l2_streamparm *parm);
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-vbi-cap.c b/drivers/media/test-drivers/vivid/vivid-vbi-cap.c
new file mode 100644
index 0000000..1a9348e
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vbi-cap.c
@@ -0,0 +1,365 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-vbi-cap.c - vbi capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-kthread-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-gen.h"
+
+static void vivid_sliced_vbi_cap_fill(struct vivid_dev *dev, unsigned seqnr)
+{
+ struct vivid_vbi_gen_data *vbi_gen = &dev->vbi_gen;
+ bool is_60hz = dev->std_cap[dev->input] & V4L2_STD_525_60;
+
+ vivid_vbi_gen_sliced(vbi_gen, is_60hz, seqnr);
+
+ if (!is_60hz) {
+ if (dev->loop_video) {
+ if (dev->vbi_out_have_wss) {
+ vbi_gen->data[12].data[0] = dev->vbi_out_wss[0];
+ vbi_gen->data[12].data[1] = dev->vbi_out_wss[1];
+ } else {
+ vbi_gen->data[12].id = 0;
+ }
+ } else {
+ switch (tpg_g_video_aspect(&dev->tpg)) {
+ case TPG_VIDEO_ASPECT_14X9_CENTRE:
+ vbi_gen->data[12].data[0] = 0x01;
+ break;
+ case TPG_VIDEO_ASPECT_16X9_CENTRE:
+ vbi_gen->data[12].data[0] = 0x0b;
+ break;
+ case TPG_VIDEO_ASPECT_16X9_ANAMORPHIC:
+ vbi_gen->data[12].data[0] = 0x07;
+ break;
+ case TPG_VIDEO_ASPECT_4X3:
+ default:
+ vbi_gen->data[12].data[0] = 0x08;
+ break;
+ }
+ }
+ } else if (dev->loop_video && is_60hz) {
+ if (dev->vbi_out_have_cc[0]) {
+ vbi_gen->data[0].data[0] = dev->vbi_out_cc[0][0];
+ vbi_gen->data[0].data[1] = dev->vbi_out_cc[0][1];
+ } else {
+ vbi_gen->data[0].id = 0;
+ }
+ if (dev->vbi_out_have_cc[1]) {
+ vbi_gen->data[1].data[0] = dev->vbi_out_cc[1][0];
+ vbi_gen->data[1].data[1] = dev->vbi_out_cc[1][1];
+ } else {
+ vbi_gen->data[1].id = 0;
+ }
+ }
+}
+
+static void vivid_g_fmt_vbi_cap(struct vivid_dev *dev, struct v4l2_vbi_format *vbi)
+{
+ bool is_60hz = dev->std_cap[dev->input] & V4L2_STD_525_60;
+
+ vbi->sampling_rate = 27000000;
+ vbi->offset = 24;
+ vbi->samples_per_line = 1440;
+ vbi->sample_format = V4L2_PIX_FMT_GREY;
+ vbi->start[0] = is_60hz ? V4L2_VBI_ITU_525_F1_START + 9 : V4L2_VBI_ITU_625_F1_START + 5;
+ vbi->start[1] = is_60hz ? V4L2_VBI_ITU_525_F2_START + 9 : V4L2_VBI_ITU_625_F2_START + 5;
+ vbi->count[0] = vbi->count[1] = is_60hz ? 12 : 18;
+ vbi->flags = dev->vbi_cap_interlaced ? V4L2_VBI_INTERLACED : 0;
+ vbi->reserved[0] = 0;
+ vbi->reserved[1] = 0;
+}
+
+void vivid_raw_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+ struct v4l2_vbi_format vbi;
+ u8 *vbuf = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+
+ vivid_g_fmt_vbi_cap(dev, &vbi);
+ buf->vb.sequence = dev->vbi_cap_seq_count;
+ if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+ buf->vb.sequence /= 2;
+
+ vivid_sliced_vbi_cap_fill(dev, buf->vb.sequence);
+
+ memset(vbuf, 0x10, vb2_plane_size(&buf->vb.vb2_buf, 0));
+
+ if (!VIVID_INVALID_SIGNAL(dev->std_signal_mode[dev->input]))
+ vivid_vbi_gen_raw(&dev->vbi_gen, &vbi, vbuf);
+}
+
+
+void vivid_sliced_vbi_cap_process(struct vivid_dev *dev,
+ struct vivid_buffer *buf)
+{
+ struct v4l2_sliced_vbi_data *vbuf =
+ vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+
+ buf->vb.sequence = dev->vbi_cap_seq_count;
+ if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+ buf->vb.sequence /= 2;
+
+ vivid_sliced_vbi_cap_fill(dev, buf->vb.sequence);
+
+ memset(vbuf, 0, vb2_plane_size(&buf->vb.vb2_buf, 0));
+ if (!VIVID_INVALID_SIGNAL(dev->std_signal_mode[dev->input])) {
+ unsigned i;
+
+ for (i = 0; i < 25; i++)
+ vbuf[i] = dev->vbi_gen.data[i];
+ }
+}
+
+static int vbi_cap_queue_setup(struct vb2_queue *vq,
+ unsigned *nbuffers, unsigned *nplanes,
+ unsigned sizes[], struct device *alloc_devs[])
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ bool is_60hz = dev->std_cap[dev->input] & V4L2_STD_525_60;
+ unsigned size = vq->type == V4L2_BUF_TYPE_SLICED_VBI_CAPTURE ?
+ 36 * sizeof(struct v4l2_sliced_vbi_data) :
+ 1440 * 2 * (is_60hz ? 12 : 18);
+
+ if (!vivid_is_sdtv_cap(dev))
+ return -EINVAL;
+
+ sizes[0] = size;
+
+ if (vq->num_buffers + *nbuffers < 2)
+ *nbuffers = 2 - vq->num_buffers;
+
+ *nplanes = 1;
+ return 0;
+}
+
+static int vbi_cap_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ bool is_60hz = dev->std_cap[dev->input] & V4L2_STD_525_60;
+ unsigned size = vb->vb2_queue->type == V4L2_BUF_TYPE_SLICED_VBI_CAPTURE ?
+ 36 * sizeof(struct v4l2_sliced_vbi_data) :
+ 1440 * 2 * (is_60hz ? 12 : 18);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->buf_prepare_error) {
+ /*
+ * Error injection: test what happens if buf_prepare() returns
+ * an error.
+ */
+ dev->buf_prepare_error = false;
+ return -EINVAL;
+ }
+ if (vb2_plane_size(vb, 0) < size) {
+ dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+ __func__, vb2_plane_size(vb, 0), size);
+ return -EINVAL;
+ }
+ vb2_set_plane_payload(vb, 0, size);
+
+ return 0;
+}
+
+static void vbi_cap_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ spin_lock(&dev->slock);
+ list_add_tail(&buf->list, &dev->vbi_cap_active);
+ spin_unlock(&dev->slock);
+}
+
+static int vbi_cap_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ int err;
+
+ dprintk(dev, 1, "%s\n", __func__);
+ dev->vbi_cap_seq_count = 0;
+ if (dev->start_streaming_error) {
+ dev->start_streaming_error = false;
+ err = -EINVAL;
+ } else {
+ err = vivid_start_generating_vid_cap(dev, &dev->vbi_cap_streaming);
+ }
+ if (err) {
+ struct vivid_buffer *buf, *tmp;
+
+ list_for_each_entry_safe(buf, tmp, &dev->vbi_cap_active, list) {
+ list_del(&buf->list);
+ vb2_buffer_done(&buf->vb.vb2_buf,
+ VB2_BUF_STATE_QUEUED);
+ }
+ }
+ return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vbi_cap_stop_streaming(struct vb2_queue *vq)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+ dprintk(dev, 1, "%s\n", __func__);
+ vivid_stop_generating_vid_cap(dev, &dev->vbi_cap_streaming);
+}
+
+static void vbi_cap_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_vbi_cap);
+}
+
+const struct vb2_ops vivid_vbi_cap_qops = {
+ .queue_setup = vbi_cap_queue_setup,
+ .buf_prepare = vbi_cap_buf_prepare,
+ .buf_queue = vbi_cap_buf_queue,
+ .start_streaming = vbi_cap_start_streaming,
+ .stop_streaming = vbi_cap_stop_streaming,
+ .buf_request_complete = vbi_cap_buf_request_complete,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_vbi_format *vbi = &f->fmt.vbi;
+
+ if (!vivid_is_sdtv_cap(dev) || !dev->has_raw_vbi_cap)
+ return -EINVAL;
+
+ vivid_g_fmt_vbi_cap(dev, vbi);
+ return 0;
+}
+
+int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ int ret = vidioc_g_fmt_vbi_cap(file, priv, f);
+
+ if (ret)
+ return ret;
+ if (dev->stream_sliced_vbi_cap && vb2_is_busy(&dev->vb_vbi_cap_q))
+ return -EBUSY;
+ dev->stream_sliced_vbi_cap = false;
+ dev->vbi_cap_dev.queue->type = V4L2_BUF_TYPE_VBI_CAPTURE;
+ return 0;
+}
+
+void vivid_fill_service_lines(struct v4l2_sliced_vbi_format *vbi, u32 service_set)
+{
+ vbi->io_size = sizeof(struct v4l2_sliced_vbi_data) * 36;
+ vbi->service_set = service_set;
+ memset(vbi->service_lines, 0, sizeof(vbi->service_lines));
+ memset(vbi->reserved, 0, sizeof(vbi->reserved));
+
+ if (vbi->service_set == 0)
+ return;
+
+ if (vbi->service_set & V4L2_SLICED_CAPTION_525) {
+ vbi->service_lines[0][21] = V4L2_SLICED_CAPTION_525;
+ vbi->service_lines[1][21] = V4L2_SLICED_CAPTION_525;
+ }
+ if (vbi->service_set & V4L2_SLICED_WSS_625) {
+ unsigned i;
+
+ for (i = 7; i <= 18; i++)
+ vbi->service_lines[0][i] =
+ vbi->service_lines[1][i] = V4L2_SLICED_TELETEXT_B;
+ vbi->service_lines[0][23] = V4L2_SLICED_WSS_625;
+ }
+}
+
+int vidioc_g_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+
+ if (!vivid_is_sdtv_cap(dev) || !dev->has_sliced_vbi_cap)
+ return -EINVAL;
+
+ vivid_fill_service_lines(vbi, dev->service_set_cap);
+ return 0;
+}
+
+int vidioc_try_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+ bool is_60hz = dev->std_cap[dev->input] & V4L2_STD_525_60;
+ u32 service_set = vbi->service_set;
+
+ if (!vivid_is_sdtv_cap(dev) || !dev->has_sliced_vbi_cap)
+ return -EINVAL;
+
+ service_set &= is_60hz ? V4L2_SLICED_CAPTION_525 :
+ V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+ vivid_fill_service_lines(vbi, service_set);
+ return 0;
+}
+
+int vidioc_s_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+ int ret = vidioc_try_fmt_sliced_vbi_cap(file, fh, fmt);
+
+ if (ret)
+ return ret;
+ if (!dev->stream_sliced_vbi_cap && vb2_is_busy(&dev->vb_vbi_cap_q))
+ return -EBUSY;
+ dev->service_set_cap = vbi->service_set;
+ dev->stream_sliced_vbi_cap = true;
+ dev->vbi_cap_dev.queue->type = V4L2_BUF_TYPE_SLICED_VBI_CAPTURE;
+ return 0;
+}
+
+int vidioc_g_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_sliced_vbi_cap *cap)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+ bool is_60hz;
+
+ if (vdev->vfl_dir == VFL_DIR_RX) {
+ is_60hz = dev->std_cap[dev->input] & V4L2_STD_525_60;
+ if (!vivid_is_sdtv_cap(dev) || !dev->has_sliced_vbi_cap ||
+ cap->type != V4L2_BUF_TYPE_SLICED_VBI_CAPTURE)
+ return -EINVAL;
+ } else {
+ is_60hz = dev->std_out & V4L2_STD_525_60;
+ if (!vivid_is_svid_out(dev) || !dev->has_sliced_vbi_out ||
+ cap->type != V4L2_BUF_TYPE_SLICED_VBI_OUTPUT)
+ return -EINVAL;
+ }
+
+ cap->service_set = is_60hz ? V4L2_SLICED_CAPTION_525 :
+ V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+ if (is_60hz) {
+ cap->service_lines[0][21] = V4L2_SLICED_CAPTION_525;
+ cap->service_lines[1][21] = V4L2_SLICED_CAPTION_525;
+ } else {
+ unsigned i;
+
+ for (i = 7; i <= 18; i++)
+ cap->service_lines[0][i] =
+ cap->service_lines[1][i] = V4L2_SLICED_TELETEXT_B;
+ cap->service_lines[0][23] = V4L2_SLICED_WSS_625;
+ }
+ return 0;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-vbi-cap.h b/drivers/media/test-drivers/vivid/vivid-vbi-cap.h
new file mode 100644
index 0000000..91d2de0
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vbi-cap.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-vbi-cap.h - vbi capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_VBI_CAP_H_
+#define _VIVID_VBI_CAP_H_
+
+void vivid_fill_time_of_day_packet(u8 *packet);
+void vivid_raw_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+void vivid_sliced_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+void vivid_sliced_vbi_out_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
+ struct v4l2_format *f);
+int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
+ struct v4l2_format *f);
+int vidioc_g_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_try_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_s_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_g_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_sliced_vbi_cap *cap);
+
+void vivid_fill_service_lines(struct v4l2_sliced_vbi_format *vbi, u32 service_set);
+
+extern const struct vb2_ops vivid_vbi_cap_qops;
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-vbi-gen.c b/drivers/media/test-drivers/vivid/vivid-vbi-gen.c
new file mode 100644
index 0000000..a141369
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vbi-gen.c
@@ -0,0 +1,311 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-vbi-gen.c - vbi generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/ktime.h>
+#include <linux/string.h>
+#include <linux/videodev2.h>
+
+#include "vivid-vbi-gen.h"
+
+static void wss_insert(u8 *wss, u32 val, unsigned size)
+{
+ while (size--)
+ *wss++ = (val & (1 << size)) ? 0xc0 : 0x10;
+}
+
+static void vivid_vbi_gen_wss_raw(const struct v4l2_sliced_vbi_data *data,
+ u8 *buf, unsigned sampling_rate)
+{
+ const unsigned rate = 5000000; /* WSS has a 5 MHz transmission rate */
+ u8 wss[29 + 24 + 24 + 24 + 18 + 18] = { 0 };
+ const unsigned zero = 0x07;
+ const unsigned one = 0x38;
+ unsigned bit = 0;
+ u16 wss_data;
+ int i;
+
+ wss_insert(wss + bit, 0x1f1c71c7, 29); bit += 29;
+ wss_insert(wss + bit, 0x1e3c1f, 24); bit += 24;
+
+ wss_data = (data->data[1] << 8) | data->data[0];
+ for (i = 0; i <= 13; i++, bit += 6)
+ wss_insert(wss + bit, (wss_data & (1 << i)) ? one : zero, 6);
+
+ for (i = 0, bit = 0; bit < sizeof(wss); bit++) {
+ unsigned n = ((bit + 1) * sampling_rate) / rate;
+
+ while (i < n)
+ buf[i++] = wss[bit];
+ }
+}
+
+static void vivid_vbi_gen_teletext_raw(const struct v4l2_sliced_vbi_data *data,
+ u8 *buf, unsigned sampling_rate)
+{
+ const unsigned rate = 6937500 / 10; /* Teletext has a 6.9375 MHz transmission rate */
+ u8 teletext[45] = { 0x55, 0x55, 0x27 };
+ unsigned bit = 0;
+ int i;
+
+ memcpy(teletext + 3, data->data, sizeof(teletext) - 3);
+ /* prevents 32 bit overflow */
+ sampling_rate /= 10;
+
+ for (i = 0, bit = 0; bit < sizeof(teletext) * 8; bit++) {
+ unsigned n = ((bit + 1) * sampling_rate) / rate;
+ u8 val = (teletext[bit / 8] & (1 << (bit & 7))) ? 0xc0 : 0x10;
+
+ while (i < n)
+ buf[i++] = val;
+ }
+}
+
+static void cc_insert(u8 *cc, u8 ch)
+{
+ unsigned tot = 0;
+ unsigned i;
+
+ for (i = 0; i < 7; i++) {
+ cc[2 * i] = cc[2 * i + 1] = (ch & (1 << i)) ? 1 : 0;
+ tot += cc[2 * i];
+ }
+ cc[14] = cc[15] = !(tot & 1);
+}
+
+#define CC_PREAMBLE_BITS (14 + 4 + 2)
+
+static void vivid_vbi_gen_cc_raw(const struct v4l2_sliced_vbi_data *data,
+ u8 *buf, unsigned sampling_rate)
+{
+ const unsigned rate = 1000000; /* CC has a 1 MHz transmission rate */
+
+ u8 cc[CC_PREAMBLE_BITS + 2 * 16] = {
+ /* Clock run-in: 7 cycles */
+ 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+ /* 2 cycles of 0 */
+ 0, 0, 0, 0,
+ /* Start bit of 1 (each bit is two cycles) */
+ 1, 1
+ };
+ unsigned bit, i;
+
+ cc_insert(cc + CC_PREAMBLE_BITS, data->data[0]);
+ cc_insert(cc + CC_PREAMBLE_BITS + 16, data->data[1]);
+
+ for (i = 0, bit = 0; bit < sizeof(cc); bit++) {
+ unsigned n = ((bit + 1) * sampling_rate) / rate;
+
+ while (i < n)
+ buf[i++] = cc[bit] ? 0xc0 : 0x10;
+ }
+}
+
+void vivid_vbi_gen_raw(const struct vivid_vbi_gen_data *vbi,
+ const struct v4l2_vbi_format *vbi_fmt, u8 *buf)
+{
+ unsigned idx;
+
+ for (idx = 0; idx < 25; idx++) {
+ const struct v4l2_sliced_vbi_data *data = vbi->data + idx;
+ unsigned start_2nd_field;
+ unsigned line = data->line;
+ u8 *linebuf = buf;
+
+ start_2nd_field = (data->id & V4L2_SLICED_VBI_525) ? 263 : 313;
+ if (data->field)
+ line += start_2nd_field;
+ line -= vbi_fmt->start[data->field];
+
+ if (vbi_fmt->flags & V4L2_VBI_INTERLACED)
+ linebuf += (line * 2 + data->field) *
+ vbi_fmt->samples_per_line;
+ else
+ linebuf += (line + data->field * vbi_fmt->count[0]) *
+ vbi_fmt->samples_per_line;
+ if (data->id == V4L2_SLICED_CAPTION_525)
+ vivid_vbi_gen_cc_raw(data, linebuf, vbi_fmt->sampling_rate);
+ else if (data->id == V4L2_SLICED_WSS_625)
+ vivid_vbi_gen_wss_raw(data, linebuf, vbi_fmt->sampling_rate);
+ else if (data->id == V4L2_SLICED_TELETEXT_B)
+ vivid_vbi_gen_teletext_raw(data, linebuf, vbi_fmt->sampling_rate);
+ }
+}
+
+static const u8 vivid_cc_sequence1[30] = {
+ 0x14, 0x20, /* Resume Caption Loading */
+ 'H', 'e',
+ 'l', 'l',
+ 'o', ' ',
+ 'w', 'o',
+ 'r', 'l',
+ 'd', '!',
+ 0x14, 0x2f, /* End of Caption */
+};
+
+static const u8 vivid_cc_sequence2[30] = {
+ 0x14, 0x20, /* Resume Caption Loading */
+ 'C', 'l',
+ 'o', 's',
+ 'e', 'd',
+ ' ', 'c',
+ 'a', 'p',
+ 't', 'i',
+ 'o', 'n',
+ 's', ' ',
+ 't', 'e',
+ 's', 't',
+ 0x14, 0x2f, /* End of Caption */
+};
+
+static u8 calc_parity(u8 val)
+{
+ unsigned i;
+ unsigned tot = 0;
+
+ for (i = 0; i < 7; i++)
+ tot += (val & (1 << i)) ? 1 : 0;
+ return val | ((tot & 1) ? 0 : 0x80);
+}
+
+static void vivid_vbi_gen_set_time_of_day(u8 *packet)
+{
+ struct tm tm;
+ u8 checksum, i;
+
+ time64_to_tm(ktime_get_real_seconds(), 0, &tm);
+ packet[0] = calc_parity(0x07);
+ packet[1] = calc_parity(0x01);
+ packet[2] = calc_parity(0x40 | tm.tm_min);
+ packet[3] = calc_parity(0x40 | tm.tm_hour);
+ packet[4] = calc_parity(0x40 | tm.tm_mday);
+ if (tm.tm_mday == 1 && tm.tm_mon == 2 &&
+ sys_tz.tz_minuteswest > tm.tm_min + tm.tm_hour * 60)
+ packet[4] = calc_parity(0x60 | tm.tm_mday);
+ packet[5] = calc_parity(0x40 | (1 + tm.tm_mon));
+ packet[6] = calc_parity(0x40 | (1 + tm.tm_wday));
+ packet[7] = calc_parity(0x40 | ((tm.tm_year - 90) & 0x3f));
+ packet[8] = calc_parity(0x0f);
+ for (checksum = i = 0; i <= 8; i++)
+ checksum += packet[i] & 0x7f;
+ packet[9] = calc_parity(0x100 - checksum);
+ checksum = 0;
+ packet[10] = calc_parity(0x07);
+ packet[11] = calc_parity(0x04);
+ if (sys_tz.tz_minuteswest >= 0)
+ packet[12] = calc_parity(0x40 | ((sys_tz.tz_minuteswest / 60) & 0x1f));
+ else
+ packet[12] = calc_parity(0x40 | ((24 + sys_tz.tz_minuteswest / 60) & 0x1f));
+ packet[13] = calc_parity(0);
+ packet[14] = calc_parity(0x0f);
+ for (checksum = 0, i = 10; i <= 14; i++)
+ checksum += packet[i] & 0x7f;
+ packet[15] = calc_parity(0x100 - checksum);
+}
+
+static const u8 hamming[16] = {
+ 0x15, 0x02, 0x49, 0x5e, 0x64, 0x73, 0x38, 0x2f,
+ 0xd0, 0xc7, 0x8c, 0x9b, 0xa1, 0xb6, 0xfd, 0xea
+};
+
+static void vivid_vbi_gen_teletext(u8 *packet, unsigned line, unsigned frame)
+{
+ unsigned offset = 2;
+ unsigned i;
+
+ packet[0] = hamming[1 + ((line & 1) << 3)];
+ packet[1] = hamming[line >> 1];
+ memset(packet + 2, 0x20, 40);
+ if (line == 0) {
+ /* subcode */
+ packet[2] = hamming[frame % 10];
+ packet[3] = hamming[frame / 10];
+ packet[4] = hamming[0];
+ packet[5] = hamming[0];
+ packet[6] = hamming[0];
+ packet[7] = hamming[0];
+ packet[8] = hamming[0];
+ packet[9] = hamming[1];
+ offset = 10;
+ }
+ packet += offset;
+ memcpy(packet, "Page: 100 Row: 10", 17);
+ packet[7] = '0' + frame / 10;
+ packet[8] = '0' + frame % 10;
+ packet[15] = '0' + line / 10;
+ packet[16] = '0' + line % 10;
+ for (i = 0; i < 42 - offset; i++)
+ packet[i] = calc_parity(packet[i]);
+}
+
+void vivid_vbi_gen_sliced(struct vivid_vbi_gen_data *vbi,
+ bool is_60hz, unsigned seqnr)
+{
+ struct v4l2_sliced_vbi_data *data0 = vbi->data;
+ struct v4l2_sliced_vbi_data *data1 = vbi->data + 1;
+ unsigned frame = seqnr % 60;
+
+ memset(vbi->data, 0, sizeof(vbi->data));
+
+ if (!is_60hz) {
+ unsigned i;
+
+ for (i = 0; i <= 11; i++) {
+ data0->id = V4L2_SLICED_TELETEXT_B;
+ data0->line = 7 + i;
+ vivid_vbi_gen_teletext(data0->data, i, frame);
+ data0++;
+ }
+ data0->id = V4L2_SLICED_WSS_625;
+ data0->line = 23;
+ /* 4x3 video aspect ratio */
+ data0->data[0] = 0x08;
+ data0++;
+ for (i = 0; i <= 11; i++) {
+ data0->id = V4L2_SLICED_TELETEXT_B;
+ data0->field = 1;
+ data0->line = 7 + i;
+ vivid_vbi_gen_teletext(data0->data, 12 + i, frame);
+ data0++;
+ }
+ return;
+ }
+
+ data0->id = V4L2_SLICED_CAPTION_525;
+ data0->line = 21;
+ data1->id = V4L2_SLICED_CAPTION_525;
+ data1->field = 1;
+ data1->line = 21;
+
+ if (frame < 15) {
+ data0->data[0] = calc_parity(vivid_cc_sequence1[2 * frame]);
+ data0->data[1] = calc_parity(vivid_cc_sequence1[2 * frame + 1]);
+ } else if (frame >= 30 && frame < 45) {
+ frame -= 30;
+ data0->data[0] = calc_parity(vivid_cc_sequence2[2 * frame]);
+ data0->data[1] = calc_parity(vivid_cc_sequence2[2 * frame + 1]);
+ } else {
+ data0->data[0] = calc_parity(0);
+ data0->data[1] = calc_parity(0);
+ }
+
+ frame = seqnr % (30 * 60);
+ switch (frame) {
+ case 0:
+ vivid_vbi_gen_set_time_of_day(vbi->time_of_day_packet);
+ fallthrough;
+ case 1 ... 7:
+ data1->data[0] = vbi->time_of_day_packet[frame * 2];
+ data1->data[1] = vbi->time_of_day_packet[frame * 2 + 1];
+ break;
+ default:
+ data1->data[0] = calc_parity(0);
+ data1->data[1] = calc_parity(0);
+ break;
+ }
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-vbi-gen.h b/drivers/media/test-drivers/vivid/vivid-vbi-gen.h
new file mode 100644
index 0000000..2657a7f
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vbi-gen.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-vbi-gen.h - vbi generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_VBI_GEN_H_
+#define _VIVID_VBI_GEN_H_
+
+struct vivid_vbi_gen_data {
+ struct v4l2_sliced_vbi_data data[25];
+ u8 time_of_day_packet[16];
+};
+
+void vivid_vbi_gen_sliced(struct vivid_vbi_gen_data *vbi,
+ bool is_60hz, unsigned seqnr);
+void vivid_vbi_gen_raw(const struct vivid_vbi_gen_data *vbi,
+ const struct v4l2_vbi_format *vbi_fmt, u8 *buf);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-vbi-out.c b/drivers/media/test-drivers/vivid/vivid-vbi-out.c
new file mode 100644
index 0000000..cd56476
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vbi-out.c
@@ -0,0 +1,250 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-vbi-out.c - vbi output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-kthread-out.h"
+#include "vivid-vbi-out.h"
+#include "vivid-vbi-cap.h"
+
+static int vbi_out_queue_setup(struct vb2_queue *vq,
+ unsigned *nbuffers, unsigned *nplanes,
+ unsigned sizes[], struct device *alloc_devs[])
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ bool is_60hz = dev->std_out & V4L2_STD_525_60;
+ unsigned size = vq->type == V4L2_BUF_TYPE_SLICED_VBI_OUTPUT ?
+ 36 * sizeof(struct v4l2_sliced_vbi_data) :
+ 1440 * 2 * (is_60hz ? 12 : 18);
+
+ if (!vivid_is_svid_out(dev))
+ return -EINVAL;
+
+ sizes[0] = size;
+
+ if (vq->num_buffers + *nbuffers < 2)
+ *nbuffers = 2 - vq->num_buffers;
+
+ *nplanes = 1;
+ return 0;
+}
+
+static int vbi_out_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ bool is_60hz = dev->std_out & V4L2_STD_525_60;
+ unsigned size = vb->vb2_queue->type == V4L2_BUF_TYPE_SLICED_VBI_OUTPUT ?
+ 36 * sizeof(struct v4l2_sliced_vbi_data) :
+ 1440 * 2 * (is_60hz ? 12 : 18);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->buf_prepare_error) {
+ /*
+ * Error injection: test what happens if buf_prepare() returns
+ * an error.
+ */
+ dev->buf_prepare_error = false;
+ return -EINVAL;
+ }
+ if (vb2_plane_size(vb, 0) < size) {
+ dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+ __func__, vb2_plane_size(vb, 0), size);
+ return -EINVAL;
+ }
+ vb2_set_plane_payload(vb, 0, size);
+
+ return 0;
+}
+
+static void vbi_out_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ spin_lock(&dev->slock);
+ list_add_tail(&buf->list, &dev->vbi_out_active);
+ spin_unlock(&dev->slock);
+}
+
+static int vbi_out_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ int err;
+
+ dprintk(dev, 1, "%s\n", __func__);
+ dev->vbi_out_seq_count = 0;
+ if (dev->start_streaming_error) {
+ dev->start_streaming_error = false;
+ err = -EINVAL;
+ } else {
+ err = vivid_start_generating_vid_out(dev, &dev->vbi_out_streaming);
+ }
+ if (err) {
+ struct vivid_buffer *buf, *tmp;
+
+ list_for_each_entry_safe(buf, tmp, &dev->vbi_out_active, list) {
+ list_del(&buf->list);
+ vb2_buffer_done(&buf->vb.vb2_buf,
+ VB2_BUF_STATE_QUEUED);
+ }
+ }
+ return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vbi_out_stop_streaming(struct vb2_queue *vq)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+ dprintk(dev, 1, "%s\n", __func__);
+ vivid_stop_generating_vid_out(dev, &dev->vbi_out_streaming);
+ dev->vbi_out_have_wss = false;
+ dev->vbi_out_have_cc[0] = false;
+ dev->vbi_out_have_cc[1] = false;
+}
+
+static void vbi_out_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_vbi_out);
+}
+
+const struct vb2_ops vivid_vbi_out_qops = {
+ .queue_setup = vbi_out_queue_setup,
+ .buf_prepare = vbi_out_buf_prepare,
+ .buf_queue = vbi_out_buf_queue,
+ .start_streaming = vbi_out_start_streaming,
+ .stop_streaming = vbi_out_stop_streaming,
+ .buf_request_complete = vbi_out_buf_request_complete,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+int vidioc_g_fmt_vbi_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_vbi_format *vbi = &f->fmt.vbi;
+ bool is_60hz = dev->std_out & V4L2_STD_525_60;
+
+ if (!vivid_is_svid_out(dev) || !dev->has_raw_vbi_out)
+ return -EINVAL;
+
+ vbi->sampling_rate = 25000000;
+ vbi->offset = 24;
+ vbi->samples_per_line = 1440;
+ vbi->sample_format = V4L2_PIX_FMT_GREY;
+ vbi->start[0] = is_60hz ? V4L2_VBI_ITU_525_F1_START + 9 : V4L2_VBI_ITU_625_F1_START + 5;
+ vbi->start[1] = is_60hz ? V4L2_VBI_ITU_525_F2_START + 9 : V4L2_VBI_ITU_625_F2_START + 5;
+ vbi->count[0] = vbi->count[1] = is_60hz ? 12 : 18;
+ vbi->flags = dev->vbi_cap_interlaced ? V4L2_VBI_INTERLACED : 0;
+ vbi->reserved[0] = 0;
+ vbi->reserved[1] = 0;
+ return 0;
+}
+
+int vidioc_s_fmt_vbi_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ int ret = vidioc_g_fmt_vbi_out(file, priv, f);
+
+ if (ret)
+ return ret;
+ if (vb2_is_busy(&dev->vb_vbi_out_q))
+ return -EBUSY;
+ dev->stream_sliced_vbi_out = false;
+ dev->vbi_out_dev.queue->type = V4L2_BUF_TYPE_VBI_OUTPUT;
+ return 0;
+}
+
+int vidioc_g_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+
+ if (!vivid_is_svid_out(dev) || !dev->has_sliced_vbi_out)
+ return -EINVAL;
+
+ vivid_fill_service_lines(vbi, dev->service_set_out);
+ return 0;
+}
+
+int vidioc_try_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+ bool is_60hz = dev->std_out & V4L2_STD_525_60;
+ u32 service_set = vbi->service_set;
+
+ if (!vivid_is_svid_out(dev) || !dev->has_sliced_vbi_out)
+ return -EINVAL;
+
+ service_set &= is_60hz ? V4L2_SLICED_CAPTION_525 :
+ V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+ vivid_fill_service_lines(vbi, service_set);
+ return 0;
+}
+
+int vidioc_s_fmt_sliced_vbi_out(struct file *file, void *fh,
+ struct v4l2_format *fmt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+ int ret = vidioc_try_fmt_sliced_vbi_out(file, fh, fmt);
+
+ if (ret)
+ return ret;
+ if (vb2_is_busy(&dev->vb_vbi_out_q))
+ return -EBUSY;
+ dev->service_set_out = vbi->service_set;
+ dev->stream_sliced_vbi_out = true;
+ dev->vbi_out_dev.queue->type = V4L2_BUF_TYPE_SLICED_VBI_OUTPUT;
+ return 0;
+}
+
+void vivid_sliced_vbi_out_process(struct vivid_dev *dev,
+ struct vivid_buffer *buf)
+{
+ struct v4l2_sliced_vbi_data *vbi =
+ vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
+ unsigned elems =
+ vb2_get_plane_payload(&buf->vb.vb2_buf, 0) / sizeof(*vbi);
+
+ dev->vbi_out_have_cc[0] = false;
+ dev->vbi_out_have_cc[1] = false;
+ dev->vbi_out_have_wss = false;
+ while (elems--) {
+ switch (vbi->id) {
+ case V4L2_SLICED_CAPTION_525:
+ if ((dev->std_out & V4L2_STD_525_60) && vbi->line == 21) {
+ dev->vbi_out_have_cc[!!vbi->field] = true;
+ dev->vbi_out_cc[!!vbi->field][0] = vbi->data[0];
+ dev->vbi_out_cc[!!vbi->field][1] = vbi->data[1];
+ }
+ break;
+ case V4L2_SLICED_WSS_625:
+ if ((dev->std_out & V4L2_STD_625_50) &&
+ vbi->field == 0 && vbi->line == 23) {
+ dev->vbi_out_have_wss = true;
+ dev->vbi_out_wss[0] = vbi->data[0];
+ dev->vbi_out_wss[1] = vbi->data[1];
+ }
+ break;
+ }
+ vbi++;
+ }
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-vbi-out.h b/drivers/media/test-drivers/vivid/vivid-vbi-out.h
new file mode 100644
index 0000000..7658494
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vbi-out.h
@@ -0,0 +1,22 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-vbi-out.h - vbi output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_VBI_OUT_H_
+#define _VIVID_VBI_OUT_H_
+
+void vivid_sliced_vbi_out_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+int vidioc_g_fmt_vbi_out(struct file *file, void *priv,
+ struct v4l2_format *f);
+int vidioc_s_fmt_vbi_out(struct file *file, void *priv,
+ struct v4l2_format *f);
+int vidioc_g_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_try_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_s_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt);
+
+extern const struct vb2_ops vivid_vbi_out_qops;
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-cap.c b/drivers/media/test-drivers/vivid/vivid-vid-cap.c
new file mode 100644
index 0000000..eadf28a
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vid-cap.c
@@ -0,0 +1,1944 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-vid-cap.c - video capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-rect.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-kthread-cap.h"
+#include "vivid-vid-cap.h"
+
+static const struct vivid_fmt formats_ovl[] = {
+ {
+ .fourcc = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_XRGB555, /* gggbbbbb arrrrrgg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_ARGB555, /* gggbbbbb arrrrrgg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+};
+
+/* The number of discrete webcam framesizes */
+#define VIVID_WEBCAM_SIZES 6
+/* The number of discrete webcam frameintervals */
+#define VIVID_WEBCAM_IVALS (VIVID_WEBCAM_SIZES * 2)
+
+/* Sizes must be in increasing order */
+static const struct v4l2_frmsize_discrete webcam_sizes[VIVID_WEBCAM_SIZES] = {
+ { 320, 180 },
+ { 640, 360 },
+ { 640, 480 },
+ { 1280, 720 },
+ { 1920, 1080 },
+ { 3840, 2160 },
+};
+
+/*
+ * Intervals must be in increasing order and there must be twice as many
+ * elements in this array as there are in webcam_sizes.
+ */
+static const struct v4l2_fract webcam_intervals[VIVID_WEBCAM_IVALS] = {
+ { 1, 1 },
+ { 1, 2 },
+ { 1, 4 },
+ { 1, 5 },
+ { 1, 10 },
+ { 2, 25 },
+ { 1, 15 },
+ { 1, 25 },
+ { 1, 30 },
+ { 1, 40 },
+ { 1, 50 },
+ { 1, 60 },
+};
+
+static int vid_cap_queue_setup(struct vb2_queue *vq,
+ unsigned *nbuffers, unsigned *nplanes,
+ unsigned sizes[], struct device *alloc_devs[])
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ unsigned buffers = tpg_g_buffers(&dev->tpg);
+ unsigned h = dev->fmt_cap_rect.height;
+ unsigned p;
+
+ if (dev->field_cap == V4L2_FIELD_ALTERNATE) {
+ /*
+ * You cannot use read() with FIELD_ALTERNATE since the field
+ * information (TOP/BOTTOM) cannot be passed back to the user.
+ */
+ if (vb2_fileio_is_active(vq))
+ return -EINVAL;
+ }
+
+ if (dev->queue_setup_error) {
+ /*
+ * Error injection: test what happens if queue_setup() returns
+ * an error.
+ */
+ dev->queue_setup_error = false;
+ return -EINVAL;
+ }
+ if (*nplanes) {
+ /*
+ * Check if the number of requested planes match
+ * the number of buffers in the current format. You can't mix that.
+ */
+ if (*nplanes != buffers)
+ return -EINVAL;
+ for (p = 0; p < buffers; p++) {
+ if (sizes[p] < tpg_g_line_width(&dev->tpg, p) * h +
+ dev->fmt_cap->data_offset[p])
+ return -EINVAL;
+ }
+ } else {
+ for (p = 0; p < buffers; p++)
+ sizes[p] = (tpg_g_line_width(&dev->tpg, p) * h) /
+ dev->fmt_cap->vdownsampling[p] +
+ dev->fmt_cap->data_offset[p];
+ }
+
+ if (vq->num_buffers + *nbuffers < 2)
+ *nbuffers = 2 - vq->num_buffers;
+
+ *nplanes = buffers;
+
+ dprintk(dev, 1, "%s: count=%d\n", __func__, *nbuffers);
+ for (p = 0; p < buffers; p++)
+ dprintk(dev, 1, "%s: size[%u]=%u\n", __func__, p, sizes[p]);
+
+ return 0;
+}
+
+static int vid_cap_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ unsigned long size;
+ unsigned buffers = tpg_g_buffers(&dev->tpg);
+ unsigned p;
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (WARN_ON(NULL == dev->fmt_cap))
+ return -EINVAL;
+
+ if (dev->buf_prepare_error) {
+ /*
+ * Error injection: test what happens if buf_prepare() returns
+ * an error.
+ */
+ dev->buf_prepare_error = false;
+ return -EINVAL;
+ }
+ for (p = 0; p < buffers; p++) {
+ size = (tpg_g_line_width(&dev->tpg, p) *
+ dev->fmt_cap_rect.height) /
+ dev->fmt_cap->vdownsampling[p] +
+ dev->fmt_cap->data_offset[p];
+
+ if (vb2_plane_size(vb, p) < size) {
+ dprintk(dev, 1, "%s data will not fit into plane %u (%lu < %lu)\n",
+ __func__, p, vb2_plane_size(vb, p), size);
+ return -EINVAL;
+ }
+
+ vb2_set_plane_payload(vb, p, size);
+ vb->planes[p].data_offset = dev->fmt_cap->data_offset[p];
+ }
+
+ return 0;
+}
+
+static void vid_cap_buf_finish(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct v4l2_timecode *tc = &vbuf->timecode;
+ unsigned fps = 25;
+ unsigned seq = vbuf->sequence;
+
+ if (!vivid_is_sdtv_cap(dev))
+ return;
+
+ /*
+ * Set the timecode. Rarely used, so it is interesting to
+ * test this.
+ */
+ vbuf->flags |= V4L2_BUF_FLAG_TIMECODE;
+ if (dev->std_cap[dev->input] & V4L2_STD_525_60)
+ fps = 30;
+ tc->type = (fps == 30) ? V4L2_TC_TYPE_30FPS : V4L2_TC_TYPE_25FPS;
+ tc->flags = 0;
+ tc->frames = seq % fps;
+ tc->seconds = (seq / fps) % 60;
+ tc->minutes = (seq / (60 * fps)) % 60;
+ tc->hours = (seq / (60 * 60 * fps)) % 24;
+}
+
+static void vid_cap_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ spin_lock(&dev->slock);
+ list_add_tail(&buf->list, &dev->vid_cap_active);
+ spin_unlock(&dev->slock);
+}
+
+static int vid_cap_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ unsigned i;
+ int err;
+
+ if (vb2_is_streaming(&dev->vb_vid_out_q))
+ dev->can_loop_video = vivid_vid_can_loop(dev);
+
+ dev->vid_cap_seq_count = 0;
+ dprintk(dev, 1, "%s\n", __func__);
+ for (i = 0; i < VIDEO_MAX_FRAME; i++)
+ dev->must_blank[i] = tpg_g_perc_fill(&dev->tpg) < 100;
+ if (dev->start_streaming_error) {
+ dev->start_streaming_error = false;
+ err = -EINVAL;
+ } else {
+ err = vivid_start_generating_vid_cap(dev, &dev->vid_cap_streaming);
+ }
+ if (err) {
+ struct vivid_buffer *buf, *tmp;
+
+ list_for_each_entry_safe(buf, tmp, &dev->vid_cap_active, list) {
+ list_del(&buf->list);
+ vb2_buffer_done(&buf->vb.vb2_buf,
+ VB2_BUF_STATE_QUEUED);
+ }
+ }
+ return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vid_cap_stop_streaming(struct vb2_queue *vq)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+ dprintk(dev, 1, "%s\n", __func__);
+ vivid_stop_generating_vid_cap(dev, &dev->vid_cap_streaming);
+ dev->can_loop_video = false;
+}
+
+static void vid_cap_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_vid_cap);
+}
+
+const struct vb2_ops vivid_vid_cap_qops = {
+ .queue_setup = vid_cap_queue_setup,
+ .buf_prepare = vid_cap_buf_prepare,
+ .buf_finish = vid_cap_buf_finish,
+ .buf_queue = vid_cap_buf_queue,
+ .start_streaming = vid_cap_start_streaming,
+ .stop_streaming = vid_cap_stop_streaming,
+ .buf_request_complete = vid_cap_buf_request_complete,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+/*
+ * Determine the 'picture' quality based on the current TV frequency: either
+ * COLOR for a good 'signal', GRAY (grayscale picture) for a slightly off
+ * signal or NOISE for no signal.
+ */
+void vivid_update_quality(struct vivid_dev *dev)
+{
+ unsigned freq_modulus;
+
+ if (dev->loop_video && (vivid_is_svid_cap(dev) || vivid_is_hdmi_cap(dev))) {
+ /*
+ * The 'noise' will only be replaced by the actual video
+ * if the output video matches the input video settings.
+ */
+ tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE, 0);
+ return;
+ }
+ if (vivid_is_hdmi_cap(dev) &&
+ VIVID_INVALID_SIGNAL(dev->dv_timings_signal_mode[dev->input])) {
+ tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE, 0);
+ return;
+ }
+ if (vivid_is_sdtv_cap(dev) &&
+ VIVID_INVALID_SIGNAL(dev->std_signal_mode[dev->input])) {
+ tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE, 0);
+ return;
+ }
+ if (!vivid_is_tv_cap(dev)) {
+ tpg_s_quality(&dev->tpg, TPG_QUAL_COLOR, 0);
+ return;
+ }
+
+ /*
+ * There is a fake channel every 6 MHz at 49.25, 55.25, etc.
+ * From +/- 0.25 MHz around the channel there is color, and from
+ * +/- 1 MHz there is grayscale (chroma is lost).
+ * Everywhere else it is just noise.
+ */
+ freq_modulus = (dev->tv_freq - 676 /* (43.25-1) * 16 */) % (6 * 16);
+ if (freq_modulus > 2 * 16) {
+ tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE,
+ next_pseudo_random32(dev->tv_freq ^ 0x55) & 0x3f);
+ return;
+ }
+ if (freq_modulus < 12 /*0.75 * 16*/ || freq_modulus > 20 /*1.25 * 16*/)
+ tpg_s_quality(&dev->tpg, TPG_QUAL_GRAY, 0);
+ else
+ tpg_s_quality(&dev->tpg, TPG_QUAL_COLOR, 0);
+}
+
+/*
+ * Get the current picture quality and the associated afc value.
+ */
+static enum tpg_quality vivid_get_quality(struct vivid_dev *dev, s32 *afc)
+{
+ unsigned freq_modulus;
+
+ if (afc)
+ *afc = 0;
+ if (tpg_g_quality(&dev->tpg) == TPG_QUAL_COLOR ||
+ tpg_g_quality(&dev->tpg) == TPG_QUAL_NOISE)
+ return tpg_g_quality(&dev->tpg);
+
+ /*
+ * There is a fake channel every 6 MHz at 49.25, 55.25, etc.
+ * From +/- 0.25 MHz around the channel there is color, and from
+ * +/- 1 MHz there is grayscale (chroma is lost).
+ * Everywhere else it is just gray.
+ */
+ freq_modulus = (dev->tv_freq - 676 /* (43.25-1) * 16 */) % (6 * 16);
+ if (afc)
+ *afc = freq_modulus - 1 * 16;
+ return TPG_QUAL_GRAY;
+}
+
+enum tpg_video_aspect vivid_get_video_aspect(const struct vivid_dev *dev)
+{
+ if (vivid_is_sdtv_cap(dev))
+ return dev->std_aspect_ratio[dev->input];
+
+ if (vivid_is_hdmi_cap(dev))
+ return dev->dv_timings_aspect_ratio[dev->input];
+
+ return TPG_VIDEO_ASPECT_IMAGE;
+}
+
+static enum tpg_pixel_aspect vivid_get_pixel_aspect(const struct vivid_dev *dev)
+{
+ if (vivid_is_sdtv_cap(dev))
+ return (dev->std_cap[dev->input] & V4L2_STD_525_60) ?
+ TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+ if (vivid_is_hdmi_cap(dev) &&
+ dev->src_rect.width == 720 && dev->src_rect.height <= 576)
+ return dev->src_rect.height == 480 ?
+ TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+ return TPG_PIXEL_ASPECT_SQUARE;
+}
+
+/*
+ * Called whenever the format has to be reset which can occur when
+ * changing inputs, standard, timings, etc.
+ */
+void vivid_update_format_cap(struct vivid_dev *dev, bool keep_controls)
+{
+ struct v4l2_bt_timings *bt = &dev->dv_timings_cap[dev->input].bt;
+ unsigned size;
+ u64 pixelclock;
+
+ switch (dev->input_type[dev->input]) {
+ case WEBCAM:
+ default:
+ dev->src_rect.width = webcam_sizes[dev->webcam_size_idx].width;
+ dev->src_rect.height = webcam_sizes[dev->webcam_size_idx].height;
+ dev->timeperframe_vid_cap = webcam_intervals[dev->webcam_ival_idx];
+ dev->field_cap = V4L2_FIELD_NONE;
+ tpg_s_rgb_range(&dev->tpg, V4L2_DV_RGB_RANGE_AUTO);
+ break;
+ case TV:
+ case SVID:
+ dev->field_cap = dev->tv_field_cap;
+ dev->src_rect.width = 720;
+ if (dev->std_cap[dev->input] & V4L2_STD_525_60) {
+ dev->src_rect.height = 480;
+ dev->timeperframe_vid_cap = (struct v4l2_fract) { 1001, 30000 };
+ dev->service_set_cap = V4L2_SLICED_CAPTION_525;
+ } else {
+ dev->src_rect.height = 576;
+ dev->timeperframe_vid_cap = (struct v4l2_fract) { 1000, 25000 };
+ dev->service_set_cap = V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+ }
+ tpg_s_rgb_range(&dev->tpg, V4L2_DV_RGB_RANGE_AUTO);
+ break;
+ case HDMI:
+ dev->src_rect.width = bt->width;
+ dev->src_rect.height = bt->height;
+ size = V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt);
+ if (dev->reduced_fps && can_reduce_fps(bt)) {
+ pixelclock = div_u64(bt->pixelclock * 1000, 1001);
+ bt->flags |= V4L2_DV_FL_REDUCED_FPS;
+ } else {
+ pixelclock = bt->pixelclock;
+ bt->flags &= ~V4L2_DV_FL_REDUCED_FPS;
+ }
+ dev->timeperframe_vid_cap = (struct v4l2_fract) {
+ size / 100, (u32)pixelclock / 100
+ };
+ if (bt->interlaced)
+ dev->field_cap = V4L2_FIELD_ALTERNATE;
+ else
+ dev->field_cap = V4L2_FIELD_NONE;
+
+ /*
+ * We can be called from within s_ctrl, in that case we can't
+ * set/get controls. Luckily we don't need to in that case.
+ */
+ if (keep_controls || !dev->colorspace)
+ break;
+ if (bt->flags & V4L2_DV_FL_IS_CE_VIDEO) {
+ if (bt->width == 720 && bt->height <= 576)
+ v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_170M);
+ else
+ v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_709);
+ v4l2_ctrl_s_ctrl(dev->real_rgb_range_cap, 1);
+ } else {
+ v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_SRGB);
+ v4l2_ctrl_s_ctrl(dev->real_rgb_range_cap, 0);
+ }
+ tpg_s_rgb_range(&dev->tpg, v4l2_ctrl_g_ctrl(dev->rgb_range_cap));
+ break;
+ }
+ vfree(dev->bitmap_cap);
+ dev->bitmap_cap = NULL;
+ vivid_update_quality(dev);
+ tpg_reset_source(&dev->tpg, dev->src_rect.width, dev->src_rect.height, dev->field_cap);
+ dev->crop_cap = dev->src_rect;
+ dev->crop_bounds_cap = dev->src_rect;
+ dev->compose_cap = dev->crop_cap;
+ if (V4L2_FIELD_HAS_T_OR_B(dev->field_cap))
+ dev->compose_cap.height /= 2;
+ dev->fmt_cap_rect = dev->compose_cap;
+ tpg_s_video_aspect(&dev->tpg, vivid_get_video_aspect(dev));
+ tpg_s_pixel_aspect(&dev->tpg, vivid_get_pixel_aspect(dev));
+ tpg_update_mv_step(&dev->tpg);
+}
+
+/* Map the field to something that is valid for the current input */
+static enum v4l2_field vivid_field_cap(struct vivid_dev *dev, enum v4l2_field field)
+{
+ if (vivid_is_sdtv_cap(dev)) {
+ switch (field) {
+ case V4L2_FIELD_INTERLACED_TB:
+ case V4L2_FIELD_INTERLACED_BT:
+ case V4L2_FIELD_SEQ_TB:
+ case V4L2_FIELD_SEQ_BT:
+ case V4L2_FIELD_TOP:
+ case V4L2_FIELD_BOTTOM:
+ case V4L2_FIELD_ALTERNATE:
+ return field;
+ case V4L2_FIELD_INTERLACED:
+ default:
+ return V4L2_FIELD_INTERLACED;
+ }
+ }
+ if (vivid_is_hdmi_cap(dev))
+ return dev->dv_timings_cap[dev->input].bt.interlaced ?
+ V4L2_FIELD_ALTERNATE : V4L2_FIELD_NONE;
+ return V4L2_FIELD_NONE;
+}
+
+static unsigned vivid_colorspace_cap(struct vivid_dev *dev)
+{
+ if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+ return tpg_g_colorspace(&dev->tpg);
+ return dev->colorspace_out;
+}
+
+static unsigned vivid_xfer_func_cap(struct vivid_dev *dev)
+{
+ if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+ return tpg_g_xfer_func(&dev->tpg);
+ return dev->xfer_func_out;
+}
+
+static unsigned vivid_ycbcr_enc_cap(struct vivid_dev *dev)
+{
+ if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+ return tpg_g_ycbcr_enc(&dev->tpg);
+ return dev->ycbcr_enc_out;
+}
+
+static unsigned int vivid_hsv_enc_cap(struct vivid_dev *dev)
+{
+ if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+ return tpg_g_hsv_enc(&dev->tpg);
+ return dev->hsv_enc_out;
+}
+
+static unsigned vivid_quantization_cap(struct vivid_dev *dev)
+{
+ if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+ return tpg_g_quantization(&dev->tpg);
+ return dev->quantization_out;
+}
+
+int vivid_g_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+ unsigned p;
+
+ mp->width = dev->fmt_cap_rect.width;
+ mp->height = dev->fmt_cap_rect.height;
+ mp->field = dev->field_cap;
+ mp->pixelformat = dev->fmt_cap->fourcc;
+ mp->colorspace = vivid_colorspace_cap(dev);
+ mp->xfer_func = vivid_xfer_func_cap(dev);
+ if (dev->fmt_cap->color_enc == TGP_COLOR_ENC_HSV)
+ mp->hsv_enc = vivid_hsv_enc_cap(dev);
+ else
+ mp->ycbcr_enc = vivid_ycbcr_enc_cap(dev);
+ mp->quantization = vivid_quantization_cap(dev);
+ mp->num_planes = dev->fmt_cap->buffers;
+ for (p = 0; p < mp->num_planes; p++) {
+ mp->plane_fmt[p].bytesperline = tpg_g_bytesperline(&dev->tpg, p);
+ mp->plane_fmt[p].sizeimage =
+ (tpg_g_line_width(&dev->tpg, p) * mp->height) /
+ dev->fmt_cap->vdownsampling[p] +
+ dev->fmt_cap->data_offset[p];
+ }
+ return 0;
+}
+
+int vivid_try_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+ struct v4l2_plane_pix_format *pfmt = mp->plane_fmt;
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct vivid_fmt *fmt;
+ unsigned bytesperline, max_bpl;
+ unsigned factor = 1;
+ unsigned w, h;
+ unsigned p;
+ bool user_set_csc = !!(mp->flags & V4L2_PIX_FMT_FLAG_SET_CSC);
+
+ fmt = vivid_get_format(dev, mp->pixelformat);
+ if (!fmt) {
+ dprintk(dev, 1, "Fourcc format (0x%08x) unknown.\n",
+ mp->pixelformat);
+ mp->pixelformat = V4L2_PIX_FMT_YUYV;
+ fmt = vivid_get_format(dev, mp->pixelformat);
+ }
+
+ mp->field = vivid_field_cap(dev, mp->field);
+ if (vivid_is_webcam(dev)) {
+ const struct v4l2_frmsize_discrete *sz =
+ v4l2_find_nearest_size(webcam_sizes,
+ VIVID_WEBCAM_SIZES, width,
+ height, mp->width, mp->height);
+
+ w = sz->width;
+ h = sz->height;
+ } else if (vivid_is_sdtv_cap(dev)) {
+ w = 720;
+ h = (dev->std_cap[dev->input] & V4L2_STD_525_60) ? 480 : 576;
+ } else {
+ w = dev->src_rect.width;
+ h = dev->src_rect.height;
+ }
+ if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+ factor = 2;
+ if (vivid_is_webcam(dev) ||
+ (!dev->has_scaler_cap && !dev->has_crop_cap && !dev->has_compose_cap)) {
+ mp->width = w;
+ mp->height = h / factor;
+ } else {
+ struct v4l2_rect r = { 0, 0, mp->width, mp->height * factor };
+
+ v4l2_rect_set_min_size(&r, &vivid_min_rect);
+ v4l2_rect_set_max_size(&r, &vivid_max_rect);
+ if (dev->has_scaler_cap && !dev->has_compose_cap) {
+ struct v4l2_rect max_r = { 0, 0, MAX_ZOOM * w, MAX_ZOOM * h };
+
+ v4l2_rect_set_max_size(&r, &max_r);
+ } else if (!dev->has_scaler_cap && dev->has_crop_cap && !dev->has_compose_cap) {
+ v4l2_rect_set_max_size(&r, &dev->src_rect);
+ } else if (!dev->has_scaler_cap && !dev->has_crop_cap) {
+ v4l2_rect_set_min_size(&r, &dev->src_rect);
+ }
+ mp->width = r.width;
+ mp->height = r.height / factor;
+ }
+
+ /* This driver supports custom bytesperline values */
+
+ mp->num_planes = fmt->buffers;
+ for (p = 0; p < fmt->buffers; p++) {
+ /* Calculate the minimum supported bytesperline value */
+ bytesperline = (mp->width * fmt->bit_depth[p]) >> 3;
+ /* Calculate the maximum supported bytesperline value */
+ max_bpl = (MAX_ZOOM * MAX_WIDTH * fmt->bit_depth[p]) >> 3;
+
+ if (pfmt[p].bytesperline > max_bpl)
+ pfmt[p].bytesperline = max_bpl;
+ if (pfmt[p].bytesperline < bytesperline)
+ pfmt[p].bytesperline = bytesperline;
+
+ pfmt[p].sizeimage = (pfmt[p].bytesperline * mp->height) /
+ fmt->vdownsampling[p] + fmt->data_offset[p];
+
+ memset(pfmt[p].reserved, 0, sizeof(pfmt[p].reserved));
+ }
+ for (p = fmt->buffers; p < fmt->planes; p++)
+ pfmt[0].sizeimage += (pfmt[0].bytesperline * mp->height *
+ (fmt->bit_depth[p] / fmt->vdownsampling[p])) /
+ (fmt->bit_depth[0] / fmt->vdownsampling[0]);
+
+ if (!user_set_csc || !v4l2_is_colorspace_valid(mp->colorspace))
+ mp->colorspace = vivid_colorspace_cap(dev);
+
+ if (!user_set_csc || !v4l2_is_xfer_func_valid(mp->xfer_func))
+ mp->xfer_func = vivid_xfer_func_cap(dev);
+
+ if (fmt->color_enc == TGP_COLOR_ENC_HSV) {
+ if (!user_set_csc || !v4l2_is_hsv_enc_valid(mp->hsv_enc))
+ mp->hsv_enc = vivid_hsv_enc_cap(dev);
+ } else if (fmt->color_enc == TGP_COLOR_ENC_YCBCR) {
+ if (!user_set_csc || !v4l2_is_ycbcr_enc_valid(mp->ycbcr_enc))
+ mp->ycbcr_enc = vivid_ycbcr_enc_cap(dev);
+ } else {
+ mp->ycbcr_enc = vivid_ycbcr_enc_cap(dev);
+ }
+
+ if (fmt->color_enc == TGP_COLOR_ENC_YCBCR ||
+ fmt->color_enc == TGP_COLOR_ENC_RGB) {
+ if (!user_set_csc || !v4l2_is_quant_valid(mp->quantization))
+ mp->quantization = vivid_quantization_cap(dev);
+ } else {
+ mp->quantization = vivid_quantization_cap(dev);
+ }
+
+ memset(mp->reserved, 0, sizeof(mp->reserved));
+ return 0;
+}
+
+int vivid_s_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_rect *crop = &dev->crop_cap;
+ struct v4l2_rect *compose = &dev->compose_cap;
+ struct vb2_queue *q = &dev->vb_vid_cap_q;
+ int ret = vivid_try_fmt_vid_cap(file, priv, f);
+ unsigned factor = 1;
+ unsigned p;
+ unsigned i;
+
+ if (ret < 0)
+ return ret;
+
+ if (vb2_is_busy(q)) {
+ dprintk(dev, 1, "%s device busy\n", __func__);
+ return -EBUSY;
+ }
+
+ if (dev->overlay_cap_owner && dev->fb_cap.fmt.pixelformat != mp->pixelformat) {
+ dprintk(dev, 1, "overlay is active, can't change pixelformat\n");
+ return -EBUSY;
+ }
+
+ dev->fmt_cap = vivid_get_format(dev, mp->pixelformat);
+ if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+ factor = 2;
+
+ /* Note: the webcam input doesn't support scaling, cropping or composing */
+
+ if (!vivid_is_webcam(dev) &&
+ (dev->has_scaler_cap || dev->has_crop_cap || dev->has_compose_cap)) {
+ struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+ if (dev->has_scaler_cap) {
+ if (dev->has_compose_cap)
+ v4l2_rect_map_inside(compose, &r);
+ else
+ *compose = r;
+ if (dev->has_crop_cap && !dev->has_compose_cap) {
+ struct v4l2_rect min_r = {
+ 0, 0,
+ r.width / MAX_ZOOM,
+ factor * r.height / MAX_ZOOM
+ };
+ struct v4l2_rect max_r = {
+ 0, 0,
+ r.width * MAX_ZOOM,
+ factor * r.height * MAX_ZOOM
+ };
+
+ v4l2_rect_set_min_size(crop, &min_r);
+ v4l2_rect_set_max_size(crop, &max_r);
+ v4l2_rect_map_inside(crop, &dev->crop_bounds_cap);
+ } else if (dev->has_crop_cap) {
+ struct v4l2_rect min_r = {
+ 0, 0,
+ compose->width / MAX_ZOOM,
+ factor * compose->height / MAX_ZOOM
+ };
+ struct v4l2_rect max_r = {
+ 0, 0,
+ compose->width * MAX_ZOOM,
+ factor * compose->height * MAX_ZOOM
+ };
+
+ v4l2_rect_set_min_size(crop, &min_r);
+ v4l2_rect_set_max_size(crop, &max_r);
+ v4l2_rect_map_inside(crop, &dev->crop_bounds_cap);
+ }
+ } else if (dev->has_crop_cap && !dev->has_compose_cap) {
+ r.height *= factor;
+ v4l2_rect_set_size_to(crop, &r);
+ v4l2_rect_map_inside(crop, &dev->crop_bounds_cap);
+ r = *crop;
+ r.height /= factor;
+ v4l2_rect_set_size_to(compose, &r);
+ } else if (!dev->has_crop_cap) {
+ v4l2_rect_map_inside(compose, &r);
+ } else {
+ r.height *= factor;
+ v4l2_rect_set_max_size(crop, &r);
+ v4l2_rect_map_inside(crop, &dev->crop_bounds_cap);
+ compose->top *= factor;
+ compose->height *= factor;
+ v4l2_rect_set_size_to(compose, crop);
+ v4l2_rect_map_inside(compose, &r);
+ compose->top /= factor;
+ compose->height /= factor;
+ }
+ } else if (vivid_is_webcam(dev)) {
+ /* Guaranteed to be a match */
+ for (i = 0; i < ARRAY_SIZE(webcam_sizes); i++)
+ if (webcam_sizes[i].width == mp->width &&
+ webcam_sizes[i].height == mp->height)
+ break;
+ dev->webcam_size_idx = i;
+ if (dev->webcam_ival_idx >= 2 * (VIVID_WEBCAM_SIZES - i))
+ dev->webcam_ival_idx = 2 * (VIVID_WEBCAM_SIZES - i) - 1;
+ vivid_update_format_cap(dev, false);
+ } else {
+ struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+ v4l2_rect_set_size_to(compose, &r);
+ r.height *= factor;
+ v4l2_rect_set_size_to(crop, &r);
+ }
+
+ dev->fmt_cap_rect.width = mp->width;
+ dev->fmt_cap_rect.height = mp->height;
+ tpg_s_buf_height(&dev->tpg, mp->height);
+ tpg_s_fourcc(&dev->tpg, dev->fmt_cap->fourcc);
+ for (p = 0; p < tpg_g_buffers(&dev->tpg); p++)
+ tpg_s_bytesperline(&dev->tpg, p, mp->plane_fmt[p].bytesperline);
+ dev->field_cap = mp->field;
+ if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+ tpg_s_field(&dev->tpg, V4L2_FIELD_TOP, true);
+ else
+ tpg_s_field(&dev->tpg, dev->field_cap, false);
+ tpg_s_crop_compose(&dev->tpg, &dev->crop_cap, &dev->compose_cap);
+ if (vivid_is_sdtv_cap(dev))
+ dev->tv_field_cap = mp->field;
+ tpg_update_mv_step(&dev->tpg);
+ dev->tpg.colorspace = mp->colorspace;
+ dev->tpg.xfer_func = mp->xfer_func;
+ if (dev->fmt_cap->color_enc == TGP_COLOR_ENC_YCBCR)
+ dev->tpg.ycbcr_enc = mp->ycbcr_enc;
+ else
+ dev->tpg.hsv_enc = mp->hsv_enc;
+ dev->tpg.quantization = mp->quantization;
+
+ return 0;
+}
+
+int vidioc_g_fmt_vid_cap_mplane(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!dev->multiplanar)
+ return -ENOTTY;
+ return vivid_g_fmt_vid_cap(file, priv, f);
+}
+
+int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!dev->multiplanar)
+ return -ENOTTY;
+ return vivid_try_fmt_vid_cap(file, priv, f);
+}
+
+int vidioc_s_fmt_vid_cap_mplane(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!dev->multiplanar)
+ return -ENOTTY;
+ return vivid_s_fmt_vid_cap(file, priv, f);
+}
+
+int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+ return fmt_sp2mp_func(file, priv, f, vivid_g_fmt_vid_cap);
+}
+
+int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+ return fmt_sp2mp_func(file, priv, f, vivid_try_fmt_vid_cap);
+}
+
+int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+ return fmt_sp2mp_func(file, priv, f, vivid_s_fmt_vid_cap);
+}
+
+int vivid_vid_cap_g_selection(struct file *file, void *priv,
+ struct v4l2_selection *sel)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!dev->has_crop_cap && !dev->has_compose_cap)
+ return -ENOTTY;
+ if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ return -EINVAL;
+ if (vivid_is_webcam(dev))
+ return -ENODATA;
+
+ sel->r.left = sel->r.top = 0;
+ switch (sel->target) {
+ case V4L2_SEL_TGT_CROP:
+ if (!dev->has_crop_cap)
+ return -EINVAL;
+ sel->r = dev->crop_cap;
+ break;
+ case V4L2_SEL_TGT_CROP_DEFAULT:
+ case V4L2_SEL_TGT_CROP_BOUNDS:
+ if (!dev->has_crop_cap)
+ return -EINVAL;
+ sel->r = dev->src_rect;
+ break;
+ case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+ if (!dev->has_compose_cap)
+ return -EINVAL;
+ sel->r = vivid_max_rect;
+ break;
+ case V4L2_SEL_TGT_COMPOSE:
+ if (!dev->has_compose_cap)
+ return -EINVAL;
+ sel->r = dev->compose_cap;
+ break;
+ case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+ if (!dev->has_compose_cap)
+ return -EINVAL;
+ sel->r = dev->fmt_cap_rect;
+ break;
+ default:
+ return -EINVAL;
+ }
+ return 0;
+}
+
+int vivid_vid_cap_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_rect *crop = &dev->crop_cap;
+ struct v4l2_rect *compose = &dev->compose_cap;
+ unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_cap) ? 2 : 1;
+ int ret;
+
+ if (!dev->has_crop_cap && !dev->has_compose_cap)
+ return -ENOTTY;
+ if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ return -EINVAL;
+ if (vivid_is_webcam(dev))
+ return -ENODATA;
+
+ switch (s->target) {
+ case V4L2_SEL_TGT_CROP:
+ if (!dev->has_crop_cap)
+ return -EINVAL;
+ ret = vivid_vid_adjust_sel(s->flags, &s->r);
+ if (ret)
+ return ret;
+ v4l2_rect_set_min_size(&s->r, &vivid_min_rect);
+ v4l2_rect_set_max_size(&s->r, &dev->src_rect);
+ v4l2_rect_map_inside(&s->r, &dev->crop_bounds_cap);
+ s->r.top /= factor;
+ s->r.height /= factor;
+ if (dev->has_scaler_cap) {
+ struct v4l2_rect fmt = dev->fmt_cap_rect;
+ struct v4l2_rect max_rect = {
+ 0, 0,
+ s->r.width * MAX_ZOOM,
+ s->r.height * MAX_ZOOM
+ };
+ struct v4l2_rect min_rect = {
+ 0, 0,
+ s->r.width / MAX_ZOOM,
+ s->r.height / MAX_ZOOM
+ };
+
+ v4l2_rect_set_min_size(&fmt, &min_rect);
+ if (!dev->has_compose_cap)
+ v4l2_rect_set_max_size(&fmt, &max_rect);
+ if (!v4l2_rect_same_size(&dev->fmt_cap_rect, &fmt) &&
+ vb2_is_busy(&dev->vb_vid_cap_q))
+ return -EBUSY;
+ if (dev->has_compose_cap) {
+ v4l2_rect_set_min_size(compose, &min_rect);
+ v4l2_rect_set_max_size(compose, &max_rect);
+ }
+ dev->fmt_cap_rect = fmt;
+ tpg_s_buf_height(&dev->tpg, fmt.height);
+ } else if (dev->has_compose_cap) {
+ struct v4l2_rect fmt = dev->fmt_cap_rect;
+
+ v4l2_rect_set_min_size(&fmt, &s->r);
+ if (!v4l2_rect_same_size(&dev->fmt_cap_rect, &fmt) &&
+ vb2_is_busy(&dev->vb_vid_cap_q))
+ return -EBUSY;
+ dev->fmt_cap_rect = fmt;
+ tpg_s_buf_height(&dev->tpg, fmt.height);
+ v4l2_rect_set_size_to(compose, &s->r);
+ v4l2_rect_map_inside(compose, &dev->fmt_cap_rect);
+ } else {
+ if (!v4l2_rect_same_size(&s->r, &dev->fmt_cap_rect) &&
+ vb2_is_busy(&dev->vb_vid_cap_q))
+ return -EBUSY;
+ v4l2_rect_set_size_to(&dev->fmt_cap_rect, &s->r);
+ v4l2_rect_set_size_to(compose, &s->r);
+ v4l2_rect_map_inside(compose, &dev->fmt_cap_rect);
+ tpg_s_buf_height(&dev->tpg, dev->fmt_cap_rect.height);
+ }
+ s->r.top *= factor;
+ s->r.height *= factor;
+ *crop = s->r;
+ break;
+ case V4L2_SEL_TGT_COMPOSE:
+ if (!dev->has_compose_cap)
+ return -EINVAL;
+ ret = vivid_vid_adjust_sel(s->flags, &s->r);
+ if (ret)
+ return ret;
+ v4l2_rect_set_min_size(&s->r, &vivid_min_rect);
+ v4l2_rect_set_max_size(&s->r, &dev->fmt_cap_rect);
+ if (dev->has_scaler_cap) {
+ struct v4l2_rect max_rect = {
+ 0, 0,
+ dev->src_rect.width * MAX_ZOOM,
+ (dev->src_rect.height / factor) * MAX_ZOOM
+ };
+
+ v4l2_rect_set_max_size(&s->r, &max_rect);
+ if (dev->has_crop_cap) {
+ struct v4l2_rect min_rect = {
+ 0, 0,
+ s->r.width / MAX_ZOOM,
+ (s->r.height * factor) / MAX_ZOOM
+ };
+ struct v4l2_rect max_rect = {
+ 0, 0,
+ s->r.width * MAX_ZOOM,
+ (s->r.height * factor) * MAX_ZOOM
+ };
+
+ v4l2_rect_set_min_size(crop, &min_rect);
+ v4l2_rect_set_max_size(crop, &max_rect);
+ v4l2_rect_map_inside(crop, &dev->crop_bounds_cap);
+ }
+ } else if (dev->has_crop_cap) {
+ s->r.top *= factor;
+ s->r.height *= factor;
+ v4l2_rect_set_max_size(&s->r, &dev->src_rect);
+ v4l2_rect_set_size_to(crop, &s->r);
+ v4l2_rect_map_inside(crop, &dev->crop_bounds_cap);
+ s->r.top /= factor;
+ s->r.height /= factor;
+ } else {
+ v4l2_rect_set_size_to(&s->r, &dev->src_rect);
+ s->r.height /= factor;
+ }
+ v4l2_rect_map_inside(&s->r, &dev->fmt_cap_rect);
+ if (dev->bitmap_cap && (compose->width != s->r.width ||
+ compose->height != s->r.height)) {
+ vfree(dev->bitmap_cap);
+ dev->bitmap_cap = NULL;
+ }
+ *compose = s->r;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ tpg_s_crop_compose(&dev->tpg, crop, compose);
+ return 0;
+}
+
+int vivid_vid_cap_g_pixelaspect(struct file *file, void *priv,
+ int type, struct v4l2_fract *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ return -EINVAL;
+
+ switch (vivid_get_pixel_aspect(dev)) {
+ case TPG_PIXEL_ASPECT_NTSC:
+ f->numerator = 11;
+ f->denominator = 10;
+ break;
+ case TPG_PIXEL_ASPECT_PAL:
+ f->numerator = 54;
+ f->denominator = 59;
+ break;
+ default:
+ break;
+ }
+ return 0;
+}
+
+int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct vivid_fmt *fmt;
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+
+ if (f->index >= ARRAY_SIZE(formats_ovl))
+ return -EINVAL;
+
+ fmt = &formats_ovl[f->index];
+
+ f->pixelformat = fmt->fourcc;
+ return 0;
+}
+
+int vidioc_g_fmt_vid_overlay(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct v4l2_rect *compose = &dev->compose_cap;
+ struct v4l2_window *win = &f->fmt.win;
+ unsigned clipcount = win->clipcount;
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+
+ win->w.top = dev->overlay_cap_top;
+ win->w.left = dev->overlay_cap_left;
+ win->w.width = compose->width;
+ win->w.height = compose->height;
+ win->field = dev->overlay_cap_field;
+ win->clipcount = dev->clipcount_cap;
+ if (clipcount > dev->clipcount_cap)
+ clipcount = dev->clipcount_cap;
+ if (dev->bitmap_cap == NULL)
+ win->bitmap = NULL;
+ else if (win->bitmap) {
+ if (copy_to_user(win->bitmap, dev->bitmap_cap,
+ ((compose->width + 7) / 8) * compose->height))
+ return -EFAULT;
+ }
+ if (clipcount && win->clips) {
+ if (copy_to_user(win->clips, dev->clips_cap,
+ clipcount * sizeof(dev->clips_cap[0])))
+ return -EFAULT;
+ }
+ return 0;
+}
+
+int vidioc_try_fmt_vid_overlay(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct v4l2_rect *compose = &dev->compose_cap;
+ struct v4l2_window *win = &f->fmt.win;
+ int i, j;
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+
+ win->w.left = clamp_t(int, win->w.left,
+ -dev->fb_cap.fmt.width, dev->fb_cap.fmt.width);
+ win->w.top = clamp_t(int, win->w.top,
+ -dev->fb_cap.fmt.height, dev->fb_cap.fmt.height);
+ win->w.width = compose->width;
+ win->w.height = compose->height;
+ if (win->field != V4L2_FIELD_BOTTOM && win->field != V4L2_FIELD_TOP)
+ win->field = V4L2_FIELD_ANY;
+ win->chromakey = 0;
+ win->global_alpha = 0;
+ if (win->clipcount && !win->clips)
+ win->clipcount = 0;
+ if (win->clipcount > MAX_CLIPS)
+ win->clipcount = MAX_CLIPS;
+ if (win->clipcount) {
+ if (copy_from_user(dev->try_clips_cap, win->clips,
+ win->clipcount * sizeof(dev->clips_cap[0])))
+ return -EFAULT;
+ for (i = 0; i < win->clipcount; i++) {
+ struct v4l2_rect *r = &dev->try_clips_cap[i].c;
+
+ r->top = clamp_t(s32, r->top, 0, dev->fb_cap.fmt.height - 1);
+ r->height = clamp_t(s32, r->height, 1, dev->fb_cap.fmt.height - r->top);
+ r->left = clamp_t(u32, r->left, 0, dev->fb_cap.fmt.width - 1);
+ r->width = clamp_t(u32, r->width, 1, dev->fb_cap.fmt.width - r->left);
+ }
+ /*
+ * Yeah, so sue me, it's an O(n^2) algorithm. But n is a small
+ * number and it's typically a one-time deal.
+ */
+ for (i = 0; i < win->clipcount - 1; i++) {
+ struct v4l2_rect *r1 = &dev->try_clips_cap[i].c;
+
+ for (j = i + 1; j < win->clipcount; j++) {
+ struct v4l2_rect *r2 = &dev->try_clips_cap[j].c;
+
+ if (v4l2_rect_overlap(r1, r2))
+ return -EINVAL;
+ }
+ }
+ if (copy_to_user(win->clips, dev->try_clips_cap,
+ win->clipcount * sizeof(dev->clips_cap[0])))
+ return -EFAULT;
+ }
+ return 0;
+}
+
+int vidioc_s_fmt_vid_overlay(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct v4l2_rect *compose = &dev->compose_cap;
+ struct v4l2_window *win = &f->fmt.win;
+ int ret = vidioc_try_fmt_vid_overlay(file, priv, f);
+ unsigned bitmap_size = ((compose->width + 7) / 8) * compose->height;
+ unsigned clips_size = win->clipcount * sizeof(dev->clips_cap[0]);
+ void *new_bitmap = NULL;
+
+ if (ret)
+ return ret;
+
+ if (win->bitmap) {
+ new_bitmap = vzalloc(bitmap_size);
+
+ if (new_bitmap == NULL)
+ return -ENOMEM;
+ if (copy_from_user(new_bitmap, win->bitmap, bitmap_size)) {
+ vfree(new_bitmap);
+ return -EFAULT;
+ }
+ }
+
+ dev->overlay_cap_top = win->w.top;
+ dev->overlay_cap_left = win->w.left;
+ dev->overlay_cap_field = win->field;
+ vfree(dev->bitmap_cap);
+ dev->bitmap_cap = new_bitmap;
+ dev->clipcount_cap = win->clipcount;
+ if (dev->clipcount_cap)
+ memcpy(dev->clips_cap, dev->try_clips_cap, clips_size);
+ return 0;
+}
+
+int vivid_vid_cap_overlay(struct file *file, void *fh, unsigned i)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+
+ if (i && dev->fb_vbase_cap == NULL)
+ return -EINVAL;
+
+ if (i && dev->fb_cap.fmt.pixelformat != dev->fmt_cap->fourcc) {
+ dprintk(dev, 1, "mismatch between overlay and video capture pixelformats\n");
+ return -EINVAL;
+ }
+
+ if (dev->overlay_cap_owner && dev->overlay_cap_owner != fh)
+ return -EBUSY;
+ dev->overlay_cap_owner = i ? fh : NULL;
+ return 0;
+}
+
+int vivid_vid_cap_g_fbuf(struct file *file, void *fh,
+ struct v4l2_framebuffer *a)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+
+ *a = dev->fb_cap;
+ a->capability = V4L2_FBUF_CAP_BITMAP_CLIPPING |
+ V4L2_FBUF_CAP_LIST_CLIPPING;
+ a->flags = V4L2_FBUF_FLAG_PRIMARY;
+ a->fmt.field = V4L2_FIELD_NONE;
+ a->fmt.colorspace = V4L2_COLORSPACE_SRGB;
+ a->fmt.priv = 0;
+ return 0;
+}
+
+int vivid_vid_cap_s_fbuf(struct file *file, void *fh,
+ const struct v4l2_framebuffer *a)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct vivid_fmt *fmt;
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+
+ if (!capable(CAP_SYS_ADMIN) && !capable(CAP_SYS_RAWIO))
+ return -EPERM;
+
+ if (dev->overlay_cap_owner)
+ return -EBUSY;
+
+ if (a->base == NULL) {
+ dev->fb_cap.base = NULL;
+ dev->fb_vbase_cap = NULL;
+ return 0;
+ }
+
+ if (a->fmt.width < 48 || a->fmt.height < 32)
+ return -EINVAL;
+ fmt = vivid_get_format(dev, a->fmt.pixelformat);
+ if (!fmt || !fmt->can_do_overlay)
+ return -EINVAL;
+ if (a->fmt.bytesperline < (a->fmt.width * fmt->bit_depth[0]) / 8)
+ return -EINVAL;
+ if (a->fmt.height * a->fmt.bytesperline < a->fmt.sizeimage)
+ return -EINVAL;
+
+ dev->fb_vbase_cap = phys_to_virt((unsigned long)a->base);
+ dev->fb_cap = *a;
+ dev->overlay_cap_left = clamp_t(int, dev->overlay_cap_left,
+ -dev->fb_cap.fmt.width, dev->fb_cap.fmt.width);
+ dev->overlay_cap_top = clamp_t(int, dev->overlay_cap_top,
+ -dev->fb_cap.fmt.height, dev->fb_cap.fmt.height);
+ return 0;
+}
+
+static const struct v4l2_audio vivid_audio_inputs[] = {
+ { 0, "TV", V4L2_AUDCAP_STEREO },
+ { 1, "Line-In", V4L2_AUDCAP_STEREO },
+};
+
+int vidioc_enum_input(struct file *file, void *priv,
+ struct v4l2_input *inp)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (inp->index >= dev->num_inputs)
+ return -EINVAL;
+
+ inp->type = V4L2_INPUT_TYPE_CAMERA;
+ switch (dev->input_type[inp->index]) {
+ case WEBCAM:
+ snprintf(inp->name, sizeof(inp->name), "Webcam %u",
+ dev->input_name_counter[inp->index]);
+ inp->capabilities = 0;
+ break;
+ case TV:
+ snprintf(inp->name, sizeof(inp->name), "TV %u",
+ dev->input_name_counter[inp->index]);
+ inp->type = V4L2_INPUT_TYPE_TUNER;
+ inp->std = V4L2_STD_ALL;
+ if (dev->has_audio_inputs)
+ inp->audioset = (1 << ARRAY_SIZE(vivid_audio_inputs)) - 1;
+ inp->capabilities = V4L2_IN_CAP_STD;
+ break;
+ case SVID:
+ snprintf(inp->name, sizeof(inp->name), "S-Video %u",
+ dev->input_name_counter[inp->index]);
+ inp->std = V4L2_STD_ALL;
+ if (dev->has_audio_inputs)
+ inp->audioset = (1 << ARRAY_SIZE(vivid_audio_inputs)) - 1;
+ inp->capabilities = V4L2_IN_CAP_STD;
+ break;
+ case HDMI:
+ snprintf(inp->name, sizeof(inp->name), "HDMI %u",
+ dev->input_name_counter[inp->index]);
+ inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
+ if (dev->edid_blocks == 0 ||
+ dev->dv_timings_signal_mode[dev->input] == NO_SIGNAL)
+ inp->status |= V4L2_IN_ST_NO_SIGNAL;
+ else if (dev->dv_timings_signal_mode[dev->input] == NO_LOCK ||
+ dev->dv_timings_signal_mode[dev->input] == OUT_OF_RANGE)
+ inp->status |= V4L2_IN_ST_NO_H_LOCK;
+ break;
+ }
+ if (dev->sensor_hflip)
+ inp->status |= V4L2_IN_ST_HFLIP;
+ if (dev->sensor_vflip)
+ inp->status |= V4L2_IN_ST_VFLIP;
+ if (dev->input == inp->index && vivid_is_sdtv_cap(dev)) {
+ if (dev->std_signal_mode[dev->input] == NO_SIGNAL) {
+ inp->status |= V4L2_IN_ST_NO_SIGNAL;
+ } else if (dev->std_signal_mode[dev->input] == NO_LOCK) {
+ inp->status |= V4L2_IN_ST_NO_H_LOCK;
+ } else if (vivid_is_tv_cap(dev)) {
+ switch (tpg_g_quality(&dev->tpg)) {
+ case TPG_QUAL_GRAY:
+ inp->status |= V4L2_IN_ST_COLOR_KILL;
+ break;
+ case TPG_QUAL_NOISE:
+ inp->status |= V4L2_IN_ST_NO_H_LOCK;
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ return 0;
+}
+
+int vidioc_g_input(struct file *file, void *priv, unsigned *i)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ *i = dev->input;
+ return 0;
+}
+
+int vidioc_s_input(struct file *file, void *priv, unsigned i)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_bt_timings *bt = &dev->dv_timings_cap[dev->input].bt;
+ unsigned brightness;
+
+ if (i >= dev->num_inputs)
+ return -EINVAL;
+
+ if (i == dev->input)
+ return 0;
+
+ if (vb2_is_busy(&dev->vb_vid_cap_q) ||
+ vb2_is_busy(&dev->vb_vbi_cap_q) ||
+ vb2_is_busy(&dev->vb_meta_cap_q))
+ return -EBUSY;
+
+ dev->input = i;
+ dev->vid_cap_dev.tvnorms = 0;
+ if (dev->input_type[i] == TV || dev->input_type[i] == SVID) {
+ dev->tv_audio_input = (dev->input_type[i] == TV) ? 0 : 1;
+ dev->vid_cap_dev.tvnorms = V4L2_STD_ALL;
+ }
+ dev->vbi_cap_dev.tvnorms = dev->vid_cap_dev.tvnorms;
+ dev->meta_cap_dev.tvnorms = dev->vid_cap_dev.tvnorms;
+ vivid_update_format_cap(dev, false);
+
+ if (dev->colorspace) {
+ switch (dev->input_type[i]) {
+ case WEBCAM:
+ v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_SRGB);
+ break;
+ case TV:
+ case SVID:
+ v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_170M);
+ break;
+ case HDMI:
+ if (bt->flags & V4L2_DV_FL_IS_CE_VIDEO) {
+ if (dev->src_rect.width == 720 && dev->src_rect.height <= 576)
+ v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_170M);
+ else
+ v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_709);
+ } else {
+ v4l2_ctrl_s_ctrl(dev->colorspace, VIVID_CS_SRGB);
+ }
+ break;
+ }
+ }
+
+ /*
+ * Modify the brightness range depending on the input.
+ * This makes it easy to use vivid to test if applications can
+ * handle control range modifications and is also how this is
+ * typically used in practice as different inputs may be hooked
+ * up to different receivers with different control ranges.
+ */
+ brightness = 128 * i + dev->input_brightness[i];
+ v4l2_ctrl_modify_range(dev->brightness,
+ 128 * i, 255 + 128 * i, 1, 128 + 128 * i);
+ v4l2_ctrl_s_ctrl(dev->brightness, brightness);
+
+ /* Restore per-input states. */
+ v4l2_ctrl_activate(dev->ctrl_dv_timings_signal_mode,
+ vivid_is_hdmi_cap(dev));
+ v4l2_ctrl_activate(dev->ctrl_dv_timings, vivid_is_hdmi_cap(dev) &&
+ dev->dv_timings_signal_mode[dev->input] ==
+ SELECTED_DV_TIMINGS);
+ v4l2_ctrl_activate(dev->ctrl_std_signal_mode, vivid_is_sdtv_cap(dev));
+ v4l2_ctrl_activate(dev->ctrl_standard, vivid_is_sdtv_cap(dev) &&
+ dev->std_signal_mode[dev->input]);
+
+ if (vivid_is_hdmi_cap(dev)) {
+ v4l2_ctrl_s_ctrl(dev->ctrl_dv_timings_signal_mode,
+ dev->dv_timings_signal_mode[dev->input]);
+ v4l2_ctrl_s_ctrl(dev->ctrl_dv_timings,
+ dev->query_dv_timings[dev->input]);
+ } else if (vivid_is_sdtv_cap(dev)) {
+ v4l2_ctrl_s_ctrl(dev->ctrl_std_signal_mode,
+ dev->std_signal_mode[dev->input]);
+ v4l2_ctrl_s_ctrl(dev->ctrl_standard,
+ dev->std_signal_mode[dev->input]);
+ }
+
+ return 0;
+}
+
+int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *vin)
+{
+ if (vin->index >= ARRAY_SIZE(vivid_audio_inputs))
+ return -EINVAL;
+ *vin = vivid_audio_inputs[vin->index];
+ return 0;
+}
+
+int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *vin)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_sdtv_cap(dev))
+ return -EINVAL;
+ *vin = vivid_audio_inputs[dev->tv_audio_input];
+ return 0;
+}
+
+int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *vin)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_sdtv_cap(dev))
+ return -EINVAL;
+ if (vin->index >= ARRAY_SIZE(vivid_audio_inputs))
+ return -EINVAL;
+ dev->tv_audio_input = vin->index;
+ return 0;
+}
+
+int vivid_video_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (vf->tuner != 0)
+ return -EINVAL;
+ vf->frequency = dev->tv_freq;
+ return 0;
+}
+
+int vivid_video_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (vf->tuner != 0)
+ return -EINVAL;
+ dev->tv_freq = clamp_t(unsigned, vf->frequency, MIN_TV_FREQ, MAX_TV_FREQ);
+ if (vivid_is_tv_cap(dev))
+ vivid_update_quality(dev);
+ return 0;
+}
+
+int vivid_video_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (vt->index != 0)
+ return -EINVAL;
+ if (vt->audmode > V4L2_TUNER_MODE_LANG1_LANG2)
+ return -EINVAL;
+ dev->tv_audmode = vt->audmode;
+ return 0;
+}
+
+int vivid_video_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ enum tpg_quality qual;
+
+ if (vt->index != 0)
+ return -EINVAL;
+
+ vt->capability = V4L2_TUNER_CAP_NORM | V4L2_TUNER_CAP_STEREO |
+ V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2;
+ vt->audmode = dev->tv_audmode;
+ vt->rangelow = MIN_TV_FREQ;
+ vt->rangehigh = MAX_TV_FREQ;
+ qual = vivid_get_quality(dev, &vt->afc);
+ if (qual == TPG_QUAL_COLOR)
+ vt->signal = 0xffff;
+ else if (qual == TPG_QUAL_GRAY)
+ vt->signal = 0x8000;
+ else
+ vt->signal = 0;
+ if (qual == TPG_QUAL_NOISE) {
+ vt->rxsubchans = 0;
+ } else if (qual == TPG_QUAL_GRAY) {
+ vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+ } else {
+ unsigned int channel_nr = dev->tv_freq / (6 * 16);
+ unsigned int options =
+ (dev->std_cap[dev->input] & V4L2_STD_NTSC_M) ? 4 : 3;
+
+ switch (channel_nr % options) {
+ case 0:
+ vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+ break;
+ case 1:
+ vt->rxsubchans = V4L2_TUNER_SUB_STEREO;
+ break;
+ case 2:
+ if (dev->std_cap[dev->input] & V4L2_STD_NTSC_M)
+ vt->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_SAP;
+ else
+ vt->rxsubchans = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+ break;
+ case 3:
+ vt->rxsubchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_SAP;
+ break;
+ }
+ }
+ strscpy(vt->name, "TV Tuner", sizeof(vt->name));
+ return 0;
+}
+
+/* Must remain in sync with the vivid_ctrl_standard_strings array */
+const v4l2_std_id vivid_standard[] = {
+ V4L2_STD_NTSC_M,
+ V4L2_STD_NTSC_M_JP,
+ V4L2_STD_NTSC_M_KR,
+ V4L2_STD_NTSC_443,
+ V4L2_STD_PAL_BG | V4L2_STD_PAL_H,
+ V4L2_STD_PAL_I,
+ V4L2_STD_PAL_DK,
+ V4L2_STD_PAL_M,
+ V4L2_STD_PAL_N,
+ V4L2_STD_PAL_Nc,
+ V4L2_STD_PAL_60,
+ V4L2_STD_SECAM_B | V4L2_STD_SECAM_G | V4L2_STD_SECAM_H,
+ V4L2_STD_SECAM_DK,
+ V4L2_STD_SECAM_L,
+ V4L2_STD_SECAM_LC,
+ V4L2_STD_UNKNOWN
+};
+
+/* Must remain in sync with the vivid_standard array */
+const char * const vivid_ctrl_standard_strings[] = {
+ "NTSC-M",
+ "NTSC-M-JP",
+ "NTSC-M-KR",
+ "NTSC-443",
+ "PAL-BGH",
+ "PAL-I",
+ "PAL-DK",
+ "PAL-M",
+ "PAL-N",
+ "PAL-Nc",
+ "PAL-60",
+ "SECAM-BGH",
+ "SECAM-DK",
+ "SECAM-L",
+ "SECAM-Lc",
+ NULL,
+};
+
+int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *id)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ unsigned int last = dev->query_std_last[dev->input];
+
+ if (!vivid_is_sdtv_cap(dev))
+ return -ENODATA;
+ if (dev->std_signal_mode[dev->input] == NO_SIGNAL ||
+ dev->std_signal_mode[dev->input] == NO_LOCK) {
+ *id = V4L2_STD_UNKNOWN;
+ return 0;
+ }
+ if (vivid_is_tv_cap(dev) && tpg_g_quality(&dev->tpg) == TPG_QUAL_NOISE) {
+ *id = V4L2_STD_UNKNOWN;
+ } else if (dev->std_signal_mode[dev->input] == CURRENT_STD) {
+ *id = dev->std_cap[dev->input];
+ } else if (dev->std_signal_mode[dev->input] == SELECTED_STD) {
+ *id = dev->query_std[dev->input];
+ } else {
+ *id = vivid_standard[last];
+ dev->query_std_last[dev->input] =
+ (last + 1) % ARRAY_SIZE(vivid_standard);
+ }
+
+ return 0;
+}
+
+int vivid_vid_cap_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_sdtv_cap(dev))
+ return -ENODATA;
+ if (dev->std_cap[dev->input] == id)
+ return 0;
+ if (vb2_is_busy(&dev->vb_vid_cap_q) || vb2_is_busy(&dev->vb_vbi_cap_q))
+ return -EBUSY;
+ dev->std_cap[dev->input] = id;
+ vivid_update_format_cap(dev, false);
+ return 0;
+}
+
+static void find_aspect_ratio(u32 width, u32 height,
+ u32 *num, u32 *denom)
+{
+ if (!(height % 3) && ((height * 4 / 3) == width)) {
+ *num = 4;
+ *denom = 3;
+ } else if (!(height % 9) && ((height * 16 / 9) == width)) {
+ *num = 16;
+ *denom = 9;
+ } else if (!(height % 10) && ((height * 16 / 10) == width)) {
+ *num = 16;
+ *denom = 10;
+ } else if (!(height % 4) && ((height * 5 / 4) == width)) {
+ *num = 5;
+ *denom = 4;
+ } else if (!(height % 9) && ((height * 15 / 9) == width)) {
+ *num = 15;
+ *denom = 9;
+ } else { /* default to 16:9 */
+ *num = 16;
+ *denom = 9;
+ }
+}
+
+static bool valid_cvt_gtf_timings(struct v4l2_dv_timings *timings)
+{
+ struct v4l2_bt_timings *bt = &timings->bt;
+ u32 total_h_pixel;
+ u32 total_v_lines;
+ u32 h_freq;
+
+ if (!v4l2_valid_dv_timings(timings, &vivid_dv_timings_cap,
+ NULL, NULL))
+ return false;
+
+ total_h_pixel = V4L2_DV_BT_FRAME_WIDTH(bt);
+ total_v_lines = V4L2_DV_BT_FRAME_HEIGHT(bt);
+
+ h_freq = (u32)bt->pixelclock / total_h_pixel;
+
+ if (bt->standards == 0 || (bt->standards & V4L2_DV_BT_STD_CVT)) {
+ if (v4l2_detect_cvt(total_v_lines, h_freq, bt->vsync, bt->width,
+ bt->polarities, bt->interlaced, timings))
+ return true;
+ }
+
+ if (bt->standards == 0 || (bt->standards & V4L2_DV_BT_STD_GTF)) {
+ struct v4l2_fract aspect_ratio;
+
+ find_aspect_ratio(bt->width, bt->height,
+ &aspect_ratio.numerator,
+ &aspect_ratio.denominator);
+ if (v4l2_detect_gtf(total_v_lines, h_freq, bt->vsync,
+ bt->polarities, bt->interlaced,
+ aspect_ratio, timings))
+ return true;
+ }
+ return false;
+}
+
+int vivid_vid_cap_s_dv_timings(struct file *file, void *_fh,
+ struct v4l2_dv_timings *timings)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_hdmi_cap(dev))
+ return -ENODATA;
+ if (!v4l2_find_dv_timings_cap(timings, &vivid_dv_timings_cap,
+ 0, NULL, NULL) &&
+ !valid_cvt_gtf_timings(timings))
+ return -EINVAL;
+
+ if (v4l2_match_dv_timings(timings, &dev->dv_timings_cap[dev->input],
+ 0, false))
+ return 0;
+ if (vb2_is_busy(&dev->vb_vid_cap_q))
+ return -EBUSY;
+
+ dev->dv_timings_cap[dev->input] = *timings;
+ vivid_update_format_cap(dev, false);
+ return 0;
+}
+
+int vidioc_query_dv_timings(struct file *file, void *_fh,
+ struct v4l2_dv_timings *timings)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ unsigned int input = dev->input;
+ unsigned int last = dev->query_dv_timings_last[input];
+
+ if (!vivid_is_hdmi_cap(dev))
+ return -ENODATA;
+ if (dev->dv_timings_signal_mode[input] == NO_SIGNAL ||
+ dev->edid_blocks == 0)
+ return -ENOLINK;
+ if (dev->dv_timings_signal_mode[input] == NO_LOCK)
+ return -ENOLCK;
+ if (dev->dv_timings_signal_mode[input] == OUT_OF_RANGE) {
+ timings->bt.pixelclock = vivid_dv_timings_cap.bt.max_pixelclock * 2;
+ return -ERANGE;
+ }
+ if (dev->dv_timings_signal_mode[input] == CURRENT_DV_TIMINGS) {
+ *timings = dev->dv_timings_cap[input];
+ } else if (dev->dv_timings_signal_mode[input] ==
+ SELECTED_DV_TIMINGS) {
+ *timings =
+ v4l2_dv_timings_presets[dev->query_dv_timings[input]];
+ } else {
+ *timings =
+ v4l2_dv_timings_presets[last];
+ dev->query_dv_timings_last[input] =
+ (last + 1) % dev->query_dv_timings_size;
+ }
+ return 0;
+}
+
+int vidioc_s_edid(struct file *file, void *_fh,
+ struct v4l2_edid *edid)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ u16 phys_addr;
+ u32 display_present = 0;
+ unsigned int i, j;
+ int ret;
+
+ memset(edid->reserved, 0, sizeof(edid->reserved));
+ if (edid->pad >= dev->num_inputs)
+ return -EINVAL;
+ if (dev->input_type[edid->pad] != HDMI || edid->start_block)
+ return -EINVAL;
+ if (edid->blocks == 0) {
+ dev->edid_blocks = 0;
+ v4l2_ctrl_s_ctrl(dev->ctrl_tx_edid_present, 0);
+ v4l2_ctrl_s_ctrl(dev->ctrl_tx_hotplug, 0);
+ phys_addr = CEC_PHYS_ADDR_INVALID;
+ goto set_phys_addr;
+ }
+ if (edid->blocks > dev->edid_max_blocks) {
+ edid->blocks = dev->edid_max_blocks;
+ return -E2BIG;
+ }
+ phys_addr = cec_get_edid_phys_addr(edid->edid, edid->blocks * 128, NULL);
+ ret = v4l2_phys_addr_validate(phys_addr, &phys_addr, NULL);
+ if (ret)
+ return ret;
+
+ if (vb2_is_busy(&dev->vb_vid_cap_q))
+ return -EBUSY;
+
+ dev->edid_blocks = edid->blocks;
+ memcpy(dev->edid, edid->edid, edid->blocks * 128);
+
+ for (i = 0, j = 0; i < dev->num_outputs; i++)
+ if (dev->output_type[i] == HDMI)
+ display_present |=
+ dev->display_present[i] << j++;
+
+ v4l2_ctrl_s_ctrl(dev->ctrl_tx_edid_present, display_present);
+ v4l2_ctrl_s_ctrl(dev->ctrl_tx_hotplug, display_present);
+
+set_phys_addr:
+ /* TODO: a proper hotplug detect cycle should be emulated here */
+ cec_s_phys_addr(dev->cec_rx_adap, phys_addr, false);
+
+ for (i = 0; i < MAX_OUTPUTS && dev->cec_tx_adap[i]; i++)
+ cec_s_phys_addr(dev->cec_tx_adap[i],
+ dev->display_present[i] ?
+ v4l2_phys_addr_for_input(phys_addr, i + 1) :
+ CEC_PHYS_ADDR_INVALID,
+ false);
+ return 0;
+}
+
+int vidioc_enum_framesizes(struct file *file, void *fh,
+ struct v4l2_frmsizeenum *fsize)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_webcam(dev) && !dev->has_scaler_cap)
+ return -EINVAL;
+ if (vivid_get_format(dev, fsize->pixel_format) == NULL)
+ return -EINVAL;
+ if (vivid_is_webcam(dev)) {
+ if (fsize->index >= ARRAY_SIZE(webcam_sizes))
+ return -EINVAL;
+ fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+ fsize->discrete = webcam_sizes[fsize->index];
+ return 0;
+ }
+ if (fsize->index)
+ return -EINVAL;
+ fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+ fsize->stepwise.min_width = MIN_WIDTH;
+ fsize->stepwise.max_width = MAX_WIDTH * MAX_ZOOM;
+ fsize->stepwise.step_width = 2;
+ fsize->stepwise.min_height = MIN_HEIGHT;
+ fsize->stepwise.max_height = MAX_HEIGHT * MAX_ZOOM;
+ fsize->stepwise.step_height = 2;
+ return 0;
+}
+
+/* timeperframe is arbitrary and continuous */
+int vidioc_enum_frameintervals(struct file *file, void *priv,
+ struct v4l2_frmivalenum *fival)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct vivid_fmt *fmt;
+ int i;
+
+ fmt = vivid_get_format(dev, fival->pixel_format);
+ if (!fmt)
+ return -EINVAL;
+
+ if (!vivid_is_webcam(dev)) {
+ if (fival->index)
+ return -EINVAL;
+ if (fival->width < MIN_WIDTH || fival->width > MAX_WIDTH * MAX_ZOOM)
+ return -EINVAL;
+ if (fival->height < MIN_HEIGHT || fival->height > MAX_HEIGHT * MAX_ZOOM)
+ return -EINVAL;
+ fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+ fival->discrete = dev->timeperframe_vid_cap;
+ return 0;
+ }
+
+ for (i = 0; i < ARRAY_SIZE(webcam_sizes); i++)
+ if (fival->width == webcam_sizes[i].width &&
+ fival->height == webcam_sizes[i].height)
+ break;
+ if (i == ARRAY_SIZE(webcam_sizes))
+ return -EINVAL;
+ if (fival->index >= 2 * (VIVID_WEBCAM_SIZES - i))
+ return -EINVAL;
+ fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+ fival->discrete = webcam_intervals[fival->index];
+ return 0;
+}
+
+int vivid_vid_cap_g_parm(struct file *file, void *priv,
+ struct v4l2_streamparm *parm)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (parm->type != (dev->multiplanar ?
+ V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+ V4L2_BUF_TYPE_VIDEO_CAPTURE))
+ return -EINVAL;
+
+ parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+ parm->parm.capture.timeperframe = dev->timeperframe_vid_cap;
+ parm->parm.capture.readbuffers = 1;
+ return 0;
+}
+
+int vivid_vid_cap_s_parm(struct file *file, void *priv,
+ struct v4l2_streamparm *parm)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ unsigned ival_sz = 2 * (VIVID_WEBCAM_SIZES - dev->webcam_size_idx);
+ struct v4l2_fract tpf;
+ unsigned i;
+
+ if (parm->type != (dev->multiplanar ?
+ V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+ V4L2_BUF_TYPE_VIDEO_CAPTURE))
+ return -EINVAL;
+ if (!vivid_is_webcam(dev))
+ return vivid_vid_cap_g_parm(file, priv, parm);
+
+ tpf = parm->parm.capture.timeperframe;
+
+ if (tpf.denominator == 0)
+ tpf = webcam_intervals[ival_sz - 1];
+ for (i = 0; i < ival_sz; i++)
+ if (V4L2_FRACT_COMPARE(tpf, >=, webcam_intervals[i]))
+ break;
+ if (i == ival_sz)
+ i = ival_sz - 1;
+ dev->webcam_ival_idx = i;
+ tpf = webcam_intervals[dev->webcam_ival_idx];
+
+ /* resync the thread's timings */
+ dev->cap_seq_resync = true;
+ dev->timeperframe_vid_cap = tpf;
+ parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+ parm->parm.capture.timeperframe = tpf;
+ parm->parm.capture.readbuffers = 1;
+ return 0;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-cap.h b/drivers/media/test-drivers/vivid/vivid-vid-cap.h
new file mode 100644
index 0000000..1e422a5
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vid-cap.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-vid-cap.h - video capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_VID_CAP_H_
+#define _VIVID_VID_CAP_H_
+
+void vivid_update_quality(struct vivid_dev *dev);
+void vivid_update_format_cap(struct vivid_dev *dev, bool keep_controls);
+enum tpg_video_aspect vivid_get_video_aspect(const struct vivid_dev *dev);
+
+extern const v4l2_std_id vivid_standard[];
+extern const char * const vivid_ctrl_standard_strings[];
+
+extern const struct vb2_ops vivid_vid_cap_qops;
+
+int vivid_g_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_try_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_s_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_cap_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_cap_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_cap_g_selection(struct file *file, void *priv, struct v4l2_selection *sel);
+int vivid_vid_cap_s_selection(struct file *file, void *fh, struct v4l2_selection *s);
+int vivid_vid_cap_g_pixelaspect(struct file *file, void *priv, int type, struct v4l2_fract *f);
+int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_cap_overlay(struct file *file, void *fh, unsigned i);
+int vivid_vid_cap_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *a);
+int vivid_vid_cap_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *a);
+int vidioc_enum_input(struct file *file, void *priv, struct v4l2_input *inp);
+int vidioc_g_input(struct file *file, void *priv, unsigned *i);
+int vidioc_s_input(struct file *file, void *priv, unsigned i);
+int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *vin);
+int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *vin);
+int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *vin);
+int vivid_video_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+int vivid_video_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
+int vivid_video_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
+int vivid_video_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
+int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *id);
+int vivid_vid_cap_s_std(struct file *file, void *priv, v4l2_std_id id);
+int vivid_vid_cap_s_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vidioc_query_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vidioc_s_edid(struct file *file, void *_fh, struct v4l2_edid *edid);
+int vidioc_enum_framesizes(struct file *file, void *fh, struct v4l2_frmsizeenum *fsize);
+int vidioc_enum_frameintervals(struct file *file, void *priv, struct v4l2_frmivalenum *fival);
+int vivid_vid_cap_g_parm(struct file *file, void *priv, struct v4l2_streamparm *parm);
+int vivid_vid_cap_s_parm(struct file *file, void *priv, struct v4l2_streamparm *parm);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-common.c b/drivers/media/test-drivers/vivid/vivid-vid-common.c
new file mode 100644
index 0000000..19701fe
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vid-common.c
@@ -0,0 +1,1060 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-vid-common.c - common video support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+
+const struct v4l2_dv_timings_cap vivid_dv_timings_cap = {
+ .type = V4L2_DV_BT_656_1120,
+ /* keep this initialization for compatibility with GCC < 4.4.6 */
+ .reserved = { 0 },
+ V4L2_INIT_BT_TIMINGS(16, MAX_WIDTH, 16, MAX_HEIGHT, 14000000, 775000000,
+ V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT |
+ V4L2_DV_BT_STD_CVT | V4L2_DV_BT_STD_GTF,
+ V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_INTERLACED)
+};
+
+/* ------------------------------------------------------------------
+ Basic structures
+ ------------------------------------------------------------------*/
+
+struct vivid_fmt vivid_formats[] = {
+ {
+ .fourcc = V4L2_PIX_FMT_YUYV,
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 1,
+ .buffers = 1,
+ .data_offset = { PLANE0_DATA_OFFSET },
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_UYVY,
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YVYU,
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_VYUY,
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YUV422P,
+ .vdownsampling = { 1, 1, 1 },
+ .bit_depth = { 8, 4, 4 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 3,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YUV420,
+ .vdownsampling = { 1, 2, 2 },
+ .bit_depth = { 8, 4, 4 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 3,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YVU420,
+ .vdownsampling = { 1, 2, 2 },
+ .bit_depth = { 8, 4, 4 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 3,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_NV12,
+ .vdownsampling = { 1, 2 },
+ .bit_depth = { 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_NV21,
+ .vdownsampling = { 1, 2 },
+ .bit_depth = { 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_NV16,
+ .vdownsampling = { 1, 1 },
+ .bit_depth = { 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_NV61,
+ .vdownsampling = { 1, 1 },
+ .bit_depth = { 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_NV24,
+ .vdownsampling = { 1, 1 },
+ .bit_depth = { 8, 16 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_NV42,
+ .vdownsampling = { 1, 1 },
+ .bit_depth = { 8, 16 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YUV555, /* uuuvvvvv ayyyyyuu */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x8000,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YUV565, /* uuuvvvvv yyyyyuuu */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YUV444, /* uuuuvvvv aaaayyyy */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0xf000,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YUV32, /* ayuv */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x000000ff,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_AYUV32,
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x000000ff,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_XYUV32,
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_VUYA32,
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0xff000000,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_VUYX32,
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_GREY,
+ .vdownsampling = { 1 },
+ .bit_depth = { 8 },
+ .color_enc = TGP_COLOR_ENC_LUMA,
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_Y10,
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .color_enc = TGP_COLOR_ENC_LUMA,
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_Y12,
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .color_enc = TGP_COLOR_ENC_LUMA,
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_Y16,
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .color_enc = TGP_COLOR_ENC_LUMA,
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_Y16_BE,
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .color_enc = TGP_COLOR_ENC_LUMA,
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGB332, /* rrrgggbb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 8 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGB444, /* ggggbbbb xxxxrrrr */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_XRGB444, /* ggggbbbb xxxxrrrr */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_ARGB444, /* ggggbbbb aaaarrrr */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x00f0,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGBX444, /* bbbbxxxx rrrrgggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGBA444, /* bbbbaaaa rrrrgggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x00f0,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_XBGR444, /* ggggrrrr xxxxbbbb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_ABGR444, /* ggggrrrr aaaabbbb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x00f0,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_BGRX444, /* rrrrxxxx bbbbgggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_BGRA444, /* rrrraaaa bbbbgggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x00f0,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGB555, /* gggbbbbb xrrrrrgg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_XRGB555, /* gggbbbbb xrrrrrgg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_ARGB555, /* gggbbbbb arrrrrgg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ .alpha_mask = 0x8000,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGBX555, /* ggbbbbbx rrrrrggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGBA555, /* ggbbbbba rrrrrggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ .alpha_mask = 0x8000,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_XBGR555, /* gggrrrrr xbbbbbgg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_ABGR555, /* gggrrrrr abbbbbgg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ .alpha_mask = 0x8000,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_BGRX555, /* ggrrrrrx bbbbbggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_BGRA555, /* ggrrrrra bbbbbggg */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .can_do_overlay = true,
+ .alpha_mask = 0x8000,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGB555X, /* xrrrrrgg gggbbbbb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_XRGB555X, /* xrrrrrgg gggbbbbb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_ARGB555X, /* arrrrrgg gggbbbbb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x0080,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGB24, /* rgb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 24 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_BGR24, /* bgr */
+ .vdownsampling = { 1 },
+ .bit_depth = { 24 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_BGR666, /* bbbbbbgg ggggrrrr rrxxxxxx */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGB32, /* xrgb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_BGR32, /* bgrx */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_XRGB32, /* xrgb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_XBGR32, /* bgrx */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_ARGB32, /* argb */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x000000ff,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_ABGR32, /* bgra */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0xff000000,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGBX32, /* rgbx */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_BGRX32, /* xbgr */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_RGBA32, /* rgba */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0x000000ff,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_BGRA32, /* abgr */
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ .alpha_mask = 0xff000000,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SBGGR8, /* Bayer BG/GR */
+ .vdownsampling = { 1 },
+ .bit_depth = { 8 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SGBRG8, /* Bayer GB/RG */
+ .vdownsampling = { 1 },
+ .bit_depth = { 8 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SGRBG8, /* Bayer GR/BG */
+ .vdownsampling = { 1 },
+ .bit_depth = { 8 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SRGGB8, /* Bayer RG/GB */
+ .vdownsampling = { 1 },
+ .bit_depth = { 8 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SBGGR10, /* Bayer BG/GR */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SGBRG10, /* Bayer GB/RG */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SGRBG10, /* Bayer GR/BG */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SRGGB10, /* Bayer RG/GB */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SBGGR12, /* Bayer BG/GR */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SGBRG12, /* Bayer GB/RG */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SGRBG12, /* Bayer GR/BG */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SRGGB12, /* Bayer RG/GB */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SBGGR16, /* Bayer BG/GR */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SGBRG16, /* Bayer GB/RG */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SGRBG16, /* Bayer GR/BG */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_SRGGB16, /* Bayer RG/GB */
+ .vdownsampling = { 1 },
+ .bit_depth = { 16 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_HSV24, /* HSV 24bits */
+ .color_enc = TGP_COLOR_ENC_HSV,
+ .vdownsampling = { 1 },
+ .bit_depth = { 24 },
+ .planes = 1,
+ .buffers = 1,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_HSV32, /* HSV 32bits */
+ .color_enc = TGP_COLOR_ENC_HSV,
+ .vdownsampling = { 1 },
+ .bit_depth = { 32 },
+ .planes = 1,
+ .buffers = 1,
+ },
+
+ /* Multiplanar formats */
+
+ {
+ .fourcc = V4L2_PIX_FMT_NV16M,
+ .vdownsampling = { 1, 1 },
+ .bit_depth = { 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 2,
+ .data_offset = { PLANE0_DATA_OFFSET, 0 },
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_NV61M,
+ .vdownsampling = { 1, 1 },
+ .bit_depth = { 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 2,
+ .data_offset = { 0, PLANE0_DATA_OFFSET },
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YUV420M,
+ .vdownsampling = { 1, 2, 2 },
+ .bit_depth = { 8, 4, 4 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 3,
+ .buffers = 3,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YVU420M,
+ .vdownsampling = { 1, 2, 2 },
+ .bit_depth = { 8, 4, 4 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 3,
+ .buffers = 3,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_NV12M,
+ .vdownsampling = { 1, 2 },
+ .bit_depth = { 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 2,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_NV21M,
+ .vdownsampling = { 1, 2 },
+ .bit_depth = { 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 2,
+ .buffers = 2,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YUV422M,
+ .vdownsampling = { 1, 1, 1 },
+ .bit_depth = { 8, 4, 4 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 3,
+ .buffers = 3,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YVU422M,
+ .vdownsampling = { 1, 1, 1 },
+ .bit_depth = { 8, 4, 4 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 3,
+ .buffers = 3,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YUV444M,
+ .vdownsampling = { 1, 1, 1 },
+ .bit_depth = { 8, 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 3,
+ .buffers = 3,
+ },
+ {
+ .fourcc = V4L2_PIX_FMT_YVU444M,
+ .vdownsampling = { 1, 1, 1 },
+ .bit_depth = { 8, 8, 8 },
+ .color_enc = TGP_COLOR_ENC_YCBCR,
+ .planes = 3,
+ .buffers = 3,
+ },
+};
+
+/* There are this many multiplanar formats in the list */
+#define VIVID_MPLANAR_FORMATS 10
+
+const struct vivid_fmt *vivid_get_format(struct vivid_dev *dev, u32 pixelformat)
+{
+ const struct vivid_fmt *fmt;
+ unsigned k;
+
+ for (k = 0; k < ARRAY_SIZE(vivid_formats); k++) {
+ fmt = &vivid_formats[k];
+ if (fmt->fourcc == pixelformat)
+ if (fmt->buffers == 1 || dev->multiplanar)
+ return fmt;
+ }
+
+ return NULL;
+}
+
+bool vivid_vid_can_loop(struct vivid_dev *dev)
+{
+ if (dev->src_rect.width != dev->sink_rect.width ||
+ dev->src_rect.height != dev->sink_rect.height)
+ return false;
+ if (dev->fmt_cap->fourcc != dev->fmt_out->fourcc)
+ return false;
+ if (dev->field_cap != dev->field_out)
+ return false;
+ /*
+ * While this can be supported, it is just too much work
+ * to actually implement.
+ */
+ if (dev->field_cap == V4L2_FIELD_SEQ_TB ||
+ dev->field_cap == V4L2_FIELD_SEQ_BT)
+ return false;
+ if (vivid_is_svid_cap(dev) && vivid_is_svid_out(dev)) {
+ if (!(dev->std_cap[dev->input] & V4L2_STD_525_60) !=
+ !(dev->std_out & V4L2_STD_525_60))
+ return false;
+ return true;
+ }
+ if (vivid_is_hdmi_cap(dev) && vivid_is_hdmi_out(dev))
+ return true;
+ return false;
+}
+
+void vivid_send_source_change(struct vivid_dev *dev, unsigned type)
+{
+ struct v4l2_event ev = {
+ .type = V4L2_EVENT_SOURCE_CHANGE,
+ .u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+ };
+ unsigned i;
+
+ for (i = 0; i < dev->num_inputs; i++) {
+ ev.id = i;
+ if (dev->input_type[i] == type) {
+ if (video_is_registered(&dev->vid_cap_dev) && dev->has_vid_cap)
+ v4l2_event_queue(&dev->vid_cap_dev, &ev);
+ if (video_is_registered(&dev->vbi_cap_dev) && dev->has_vbi_cap)
+ v4l2_event_queue(&dev->vbi_cap_dev, &ev);
+ }
+ }
+}
+
+/*
+ * Conversion function that converts a single-planar format to a
+ * single-plane multiplanar format.
+ */
+void fmt_sp2mp(const struct v4l2_format *sp_fmt, struct v4l2_format *mp_fmt)
+{
+ struct v4l2_pix_format_mplane *mp = &mp_fmt->fmt.pix_mp;
+ struct v4l2_plane_pix_format *ppix = &mp->plane_fmt[0];
+ const struct v4l2_pix_format *pix = &sp_fmt->fmt.pix;
+ bool is_out = sp_fmt->type == V4L2_BUF_TYPE_VIDEO_OUTPUT;
+
+ memset(mp->reserved, 0, sizeof(mp->reserved));
+ mp_fmt->type = is_out ? V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+ V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+ mp->width = pix->width;
+ mp->height = pix->height;
+ mp->pixelformat = pix->pixelformat;
+ mp->field = pix->field;
+ mp->colorspace = pix->colorspace;
+ mp->xfer_func = pix->xfer_func;
+ /* Also copies hsv_enc */
+ mp->ycbcr_enc = pix->ycbcr_enc;
+ mp->quantization = pix->quantization;
+ mp->num_planes = 1;
+ mp->flags = pix->flags;
+ ppix->sizeimage = pix->sizeimage;
+ ppix->bytesperline = pix->bytesperline;
+ memset(ppix->reserved, 0, sizeof(ppix->reserved));
+}
+
+int fmt_sp2mp_func(struct file *file, void *priv,
+ struct v4l2_format *f, fmtfunc func)
+{
+ struct v4l2_format fmt;
+ struct v4l2_pix_format_mplane *mp = &fmt.fmt.pix_mp;
+ struct v4l2_plane_pix_format *ppix = &mp->plane_fmt[0];
+ struct v4l2_pix_format *pix = &f->fmt.pix;
+ int ret;
+
+ /* Converts to a mplane format */
+ fmt_sp2mp(f, &fmt);
+ /* Passes it to the generic mplane format function */
+ ret = func(file, priv, &fmt);
+ /* Copies back the mplane data to the single plane format */
+ pix->width = mp->width;
+ pix->height = mp->height;
+ pix->pixelformat = mp->pixelformat;
+ pix->field = mp->field;
+ pix->colorspace = mp->colorspace;
+ pix->xfer_func = mp->xfer_func;
+ /* Also copies hsv_enc */
+ pix->ycbcr_enc = mp->ycbcr_enc;
+ pix->quantization = mp->quantization;
+ pix->sizeimage = ppix->sizeimage;
+ pix->bytesperline = ppix->bytesperline;
+ pix->flags = mp->flags;
+ return ret;
+}
+
+int vivid_vid_adjust_sel(unsigned flags, struct v4l2_rect *r)
+{
+ unsigned w = r->width;
+ unsigned h = r->height;
+
+ /* sanitize w and h in case someone passes ~0 as the value */
+ w &= 0xffff;
+ h &= 0xffff;
+ if (!(flags & V4L2_SEL_FLAG_LE)) {
+ w++;
+ h++;
+ if (w < 2)
+ w = 2;
+ if (h < 2)
+ h = 2;
+ }
+ if (!(flags & V4L2_SEL_FLAG_GE)) {
+ if (w > MAX_WIDTH)
+ w = MAX_WIDTH;
+ if (h > MAX_HEIGHT)
+ h = MAX_HEIGHT;
+ }
+ w = w & ~1;
+ h = h & ~1;
+ if (w < 2 || h < 2)
+ return -ERANGE;
+ if (w > MAX_WIDTH || h > MAX_HEIGHT)
+ return -ERANGE;
+ if (r->top < 0)
+ r->top = 0;
+ if (r->left < 0)
+ r->left = 0;
+ /* sanitize left and top in case someone passes ~0 as the value */
+ r->left &= 0xfffe;
+ r->top &= 0xfffe;
+ if (r->left + w > MAX_WIDTH)
+ r->left = MAX_WIDTH - w;
+ if (r->top + h > MAX_HEIGHT)
+ r->top = MAX_HEIGHT - h;
+ if ((flags & (V4L2_SEL_FLAG_GE | V4L2_SEL_FLAG_LE)) ==
+ (V4L2_SEL_FLAG_GE | V4L2_SEL_FLAG_LE) &&
+ (r->width != w || r->height != h))
+ return -ERANGE;
+ r->width = w;
+ r->height = h;
+ return 0;
+}
+
+int vivid_enum_fmt_vid(struct file *file, void *priv,
+ struct v4l2_fmtdesc *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct vivid_fmt *fmt;
+
+ if (f->index >= ARRAY_SIZE(vivid_formats) -
+ (dev->multiplanar ? 0 : VIVID_MPLANAR_FORMATS))
+ return -EINVAL;
+
+ fmt = &vivid_formats[f->index];
+
+ f->pixelformat = fmt->fourcc;
+
+ if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+ f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
+ return 0;
+ /*
+ * For capture devices, we support the CSC API.
+ * We allow userspace to:
+ * 1. set the colorspace
+ * 2. set the xfer_func
+ * 3. set the ycbcr_enc on YUV formats
+ * 4. set the hsv_enc on HSV formats
+ * 5. set the quantization on YUV and RGB formats
+ */
+ f->flags |= V4L2_FMT_FLAG_CSC_COLORSPACE;
+ f->flags |= V4L2_FMT_FLAG_CSC_XFER_FUNC;
+
+ if (fmt->color_enc == TGP_COLOR_ENC_YCBCR) {
+ f->flags |= V4L2_FMT_FLAG_CSC_YCBCR_ENC;
+ f->flags |= V4L2_FMT_FLAG_CSC_QUANTIZATION;
+ } else if (fmt->color_enc == TGP_COLOR_ENC_HSV) {
+ f->flags |= V4L2_FMT_FLAG_CSC_HSV_ENC;
+ } else if (fmt->color_enc == TGP_COLOR_ENC_RGB) {
+ f->flags |= V4L2_FMT_FLAG_CSC_QUANTIZATION;
+ }
+
+ return 0;
+}
+
+int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX) {
+ if (!vivid_is_sdtv_cap(dev))
+ return -ENODATA;
+ *id = dev->std_cap[dev->input];
+ } else {
+ if (!vivid_is_svid_out(dev))
+ return -ENODATA;
+ *id = dev->std_out;
+ }
+ return 0;
+}
+
+int vidioc_g_dv_timings(struct file *file, void *_fh,
+ struct v4l2_dv_timings *timings)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX) {
+ if (!vivid_is_hdmi_cap(dev))
+ return -ENODATA;
+ *timings = dev->dv_timings_cap[dev->input];
+ } else {
+ if (!vivid_is_hdmi_out(dev))
+ return -ENODATA;
+ *timings = dev->dv_timings_out;
+ }
+ return 0;
+}
+
+int vidioc_enum_dv_timings(struct file *file, void *_fh,
+ struct v4l2_enum_dv_timings *timings)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX) {
+ if (!vivid_is_hdmi_cap(dev))
+ return -ENODATA;
+ } else {
+ if (!vivid_is_hdmi_out(dev))
+ return -ENODATA;
+ }
+ return v4l2_enum_dv_timings_cap(timings, &vivid_dv_timings_cap,
+ NULL, NULL);
+}
+
+int vidioc_dv_timings_cap(struct file *file, void *_fh,
+ struct v4l2_dv_timings_cap *cap)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+
+ if (vdev->vfl_dir == VFL_DIR_RX) {
+ if (!vivid_is_hdmi_cap(dev))
+ return -ENODATA;
+ } else {
+ if (!vivid_is_hdmi_out(dev))
+ return -ENODATA;
+ }
+ *cap = vivid_dv_timings_cap;
+ return 0;
+}
+
+int vidioc_g_edid(struct file *file, void *_fh,
+ struct v4l2_edid *edid)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct video_device *vdev = video_devdata(file);
+ struct cec_adapter *adap;
+
+ memset(edid->reserved, 0, sizeof(edid->reserved));
+ if (vdev->vfl_dir == VFL_DIR_RX) {
+ if (edid->pad >= dev->num_inputs)
+ return -EINVAL;
+ if (dev->input_type[edid->pad] != HDMI)
+ return -EINVAL;
+ adap = dev->cec_rx_adap;
+ } else {
+ unsigned int bus_idx;
+
+ if (edid->pad >= dev->num_outputs)
+ return -EINVAL;
+ if (dev->output_type[edid->pad] != HDMI)
+ return -EINVAL;
+ if (!dev->display_present[edid->pad])
+ return -ENODATA;
+ bus_idx = dev->cec_output2bus_map[edid->pad];
+ adap = dev->cec_tx_adap[bus_idx];
+ }
+ if (edid->start_block == 0 && edid->blocks == 0) {
+ edid->blocks = dev->edid_blocks;
+ return 0;
+ }
+ if (dev->edid_blocks == 0)
+ return -ENODATA;
+ if (edid->start_block >= dev->edid_blocks)
+ return -EINVAL;
+ if (edid->blocks > dev->edid_blocks - edid->start_block)
+ edid->blocks = dev->edid_blocks - edid->start_block;
+ if (adap)
+ v4l2_set_edid_phys_addr(dev->edid, dev->edid_blocks * 128, adap->phys_addr);
+ memcpy(edid->edid, dev->edid + edid->start_block * 128, edid->blocks * 128);
+ return 0;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-common.h b/drivers/media/test-drivers/vivid/vivid-vid-common.h
new file mode 100644
index 0000000..d908d97
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vid-common.h
@@ -0,0 +1,38 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-vid-common.h - common video support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_VID_COMMON_H_
+#define _VIVID_VID_COMMON_H_
+
+typedef int (*fmtfunc)(struct file *file, void *priv, struct v4l2_format *f);
+
+/*
+ * Conversion function that converts a single-planar format to a
+ * single-plane multiplanar format.
+ */
+void fmt_sp2mp(const struct v4l2_format *sp_fmt, struct v4l2_format *mp_fmt);
+int fmt_sp2mp_func(struct file *file, void *priv,
+ struct v4l2_format *f, fmtfunc func);
+
+extern const struct v4l2_dv_timings_cap vivid_dv_timings_cap;
+
+const struct vivid_fmt *vivid_get_format(struct vivid_dev *dev, u32 pixelformat);
+
+bool vivid_vid_can_loop(struct vivid_dev *dev);
+void vivid_send_source_change(struct vivid_dev *dev, unsigned type);
+
+int vivid_vid_adjust_sel(unsigned flags, struct v4l2_rect *r);
+
+int vivid_enum_fmt_vid(struct file *file, void *priv, struct v4l2_fmtdesc *f);
+int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id);
+int vidioc_g_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vidioc_enum_dv_timings(struct file *file, void *_fh, struct v4l2_enum_dv_timings *timings);
+int vidioc_dv_timings_cap(struct file *file, void *_fh, struct v4l2_dv_timings_cap *cap);
+int vidioc_g_edid(struct file *file, void *_fh, struct v4l2_edid *edid);
+int vidioc_subscribe_event(struct v4l2_fh *fh, const struct v4l2_event_subscription *sub);
+
+#endif
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-out.c b/drivers/media/test-drivers/vivid/vivid-vid-out.c
new file mode 100644
index 0000000..cd6c247
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vid-out.c
@@ -0,0 +1,1210 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * vivid-vid-out.c - video output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-rect.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-kthread-out.h"
+#include "vivid-vid-out.h"
+
+static int vid_out_queue_setup(struct vb2_queue *vq,
+ unsigned *nbuffers, unsigned *nplanes,
+ unsigned sizes[], struct device *alloc_devs[])
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ const struct vivid_fmt *vfmt = dev->fmt_out;
+ unsigned planes = vfmt->buffers;
+ unsigned h = dev->fmt_out_rect.height;
+ unsigned int size = dev->bytesperline_out[0] * h + vfmt->data_offset[0];
+ unsigned p;
+
+ for (p = vfmt->buffers; p < vfmt->planes; p++)
+ size += dev->bytesperline_out[p] * h / vfmt->vdownsampling[p] +
+ vfmt->data_offset[p];
+
+ if (dev->field_out == V4L2_FIELD_ALTERNATE) {
+ /*
+ * You cannot use write() with FIELD_ALTERNATE since the field
+ * information (TOP/BOTTOM) cannot be passed to the kernel.
+ */
+ if (vb2_fileio_is_active(vq))
+ return -EINVAL;
+ }
+
+ if (dev->queue_setup_error) {
+ /*
+ * Error injection: test what happens if queue_setup() returns
+ * an error.
+ */
+ dev->queue_setup_error = false;
+ return -EINVAL;
+ }
+
+ if (*nplanes) {
+ /*
+ * Check if the number of requested planes match
+ * the number of planes in the current format. You can't mix that.
+ */
+ if (*nplanes != planes)
+ return -EINVAL;
+ if (sizes[0] < size)
+ return -EINVAL;
+ for (p = 1; p < planes; p++) {
+ if (sizes[p] < dev->bytesperline_out[p] * h +
+ vfmt->data_offset[p])
+ return -EINVAL;
+ }
+ } else {
+ for (p = 0; p < planes; p++)
+ sizes[p] = p ? dev->bytesperline_out[p] * h +
+ vfmt->data_offset[p] : size;
+ }
+
+ if (vq->num_buffers + *nbuffers < 2)
+ *nbuffers = 2 - vq->num_buffers;
+
+ *nplanes = planes;
+
+ dprintk(dev, 1, "%s: count=%d\n", __func__, *nbuffers);
+ for (p = 0; p < planes; p++)
+ dprintk(dev, 1, "%s: size[%u]=%u\n", __func__, p, sizes[p]);
+ return 0;
+}
+
+static int vid_out_buf_out_validate(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (dev->field_out != V4L2_FIELD_ALTERNATE)
+ vbuf->field = dev->field_out;
+ else if (vbuf->field != V4L2_FIELD_TOP &&
+ vbuf->field != V4L2_FIELD_BOTTOM)
+ return -EINVAL;
+ return 0;
+}
+
+static int vid_out_buf_prepare(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ const struct vivid_fmt *vfmt = dev->fmt_out;
+ unsigned int planes = vfmt->buffers;
+ unsigned int h = dev->fmt_out_rect.height;
+ unsigned int size = dev->bytesperline_out[0] * h;
+ unsigned p;
+
+ for (p = vfmt->buffers; p < vfmt->planes; p++)
+ size += dev->bytesperline_out[p] * h / vfmt->vdownsampling[p];
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ if (WARN_ON(NULL == dev->fmt_out))
+ return -EINVAL;
+
+ if (dev->buf_prepare_error) {
+ /*
+ * Error injection: test what happens if buf_prepare() returns
+ * an error.
+ */
+ dev->buf_prepare_error = false;
+ return -EINVAL;
+ }
+
+ for (p = 0; p < planes; p++) {
+ if (p)
+ size = dev->bytesperline_out[p] * h;
+ size += vb->planes[p].data_offset;
+
+ if (vb2_get_plane_payload(vb, p) < size) {
+ dprintk(dev, 1, "%s the payload is too small for plane %u (%lu < %u)\n",
+ __func__, p, vb2_get_plane_payload(vb, p), size);
+ return -EINVAL;
+ }
+ }
+
+ return 0;
+}
+
+static void vid_out_buf_queue(struct vb2_buffer *vb)
+{
+ struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+ struct vivid_buffer *buf = container_of(vbuf, struct vivid_buffer, vb);
+
+ dprintk(dev, 1, "%s\n", __func__);
+
+ spin_lock(&dev->slock);
+ list_add_tail(&buf->list, &dev->vid_out_active);
+ spin_unlock(&dev->slock);
+}
+
+static int vid_out_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+ int err;
+
+ if (vb2_is_streaming(&dev->vb_vid_cap_q))
+ dev->can_loop_video = vivid_vid_can_loop(dev);
+
+ dev->vid_out_seq_count = 0;
+ dprintk(dev, 1, "%s\n", __func__);
+ if (dev->start_streaming_error) {
+ dev->start_streaming_error = false;
+ err = -EINVAL;
+ } else {
+ err = vivid_start_generating_vid_out(dev, &dev->vid_out_streaming);
+ }
+ if (err) {
+ struct vivid_buffer *buf, *tmp;
+
+ list_for_each_entry_safe(buf, tmp, &dev->vid_out_active, list) {
+ list_del(&buf->list);
+ vb2_buffer_done(&buf->vb.vb2_buf,
+ VB2_BUF_STATE_QUEUED);
+ }
+ }
+ return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vid_out_stop_streaming(struct vb2_queue *vq)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+ dprintk(dev, 1, "%s\n", __func__);
+ vivid_stop_generating_vid_out(dev, &dev->vid_out_streaming);
+ dev->can_loop_video = false;
+}
+
+static void vid_out_buf_request_complete(struct vb2_buffer *vb)
+{
+ struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+
+ v4l2_ctrl_request_complete(vb->req_obj.req, &dev->ctrl_hdl_vid_out);
+}
+
+const struct vb2_ops vivid_vid_out_qops = {
+ .queue_setup = vid_out_queue_setup,
+ .buf_out_validate = vid_out_buf_out_validate,
+ .buf_prepare = vid_out_buf_prepare,
+ .buf_queue = vid_out_buf_queue,
+ .start_streaming = vid_out_start_streaming,
+ .stop_streaming = vid_out_stop_streaming,
+ .buf_request_complete = vid_out_buf_request_complete,
+ .wait_prepare = vb2_ops_wait_prepare,
+ .wait_finish = vb2_ops_wait_finish,
+};
+
+/*
+ * Called whenever the format has to be reset which can occur when
+ * changing outputs, standard, timings, etc.
+ */
+void vivid_update_format_out(struct vivid_dev *dev)
+{
+ struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
+ unsigned size, p;
+ u64 pixelclock;
+
+ switch (dev->output_type[dev->output]) {
+ case SVID:
+ default:
+ dev->field_out = dev->tv_field_out;
+ dev->sink_rect.width = 720;
+ if (dev->std_out & V4L2_STD_525_60) {
+ dev->sink_rect.height = 480;
+ dev->timeperframe_vid_out = (struct v4l2_fract) { 1001, 30000 };
+ dev->service_set_out = V4L2_SLICED_CAPTION_525;
+ } else {
+ dev->sink_rect.height = 576;
+ dev->timeperframe_vid_out = (struct v4l2_fract) { 1000, 25000 };
+ dev->service_set_out = V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+ }
+ dev->colorspace_out = V4L2_COLORSPACE_SMPTE170M;
+ break;
+ case HDMI:
+ dev->sink_rect.width = bt->width;
+ dev->sink_rect.height = bt->height;
+ size = V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt);
+
+ if (can_reduce_fps(bt) && (bt->flags & V4L2_DV_FL_REDUCED_FPS))
+ pixelclock = div_u64(bt->pixelclock * 1000, 1001);
+ else
+ pixelclock = bt->pixelclock;
+
+ dev->timeperframe_vid_out = (struct v4l2_fract) {
+ size / 100, (u32)pixelclock / 100
+ };
+ if (bt->interlaced)
+ dev->field_out = V4L2_FIELD_ALTERNATE;
+ else
+ dev->field_out = V4L2_FIELD_NONE;
+ if (!dev->dvi_d_out && (bt->flags & V4L2_DV_FL_IS_CE_VIDEO)) {
+ if (bt->width == 720 && bt->height <= 576)
+ dev->colorspace_out = V4L2_COLORSPACE_SMPTE170M;
+ else
+ dev->colorspace_out = V4L2_COLORSPACE_REC709;
+ } else {
+ dev->colorspace_out = V4L2_COLORSPACE_SRGB;
+ }
+ break;
+ }
+ dev->xfer_func_out = V4L2_XFER_FUNC_DEFAULT;
+ dev->ycbcr_enc_out = V4L2_YCBCR_ENC_DEFAULT;
+ dev->hsv_enc_out = V4L2_HSV_ENC_180;
+ dev->quantization_out = V4L2_QUANTIZATION_DEFAULT;
+ dev->compose_out = dev->sink_rect;
+ dev->compose_bounds_out = dev->sink_rect;
+ dev->crop_out = dev->compose_out;
+ if (V4L2_FIELD_HAS_T_OR_B(dev->field_out))
+ dev->crop_out.height /= 2;
+ dev->fmt_out_rect = dev->crop_out;
+ for (p = 0; p < dev->fmt_out->planes; p++)
+ dev->bytesperline_out[p] =
+ (dev->sink_rect.width * dev->fmt_out->bit_depth[p]) / 8;
+}
+
+/* Map the field to something that is valid for the current output */
+static enum v4l2_field vivid_field_out(struct vivid_dev *dev, enum v4l2_field field)
+{
+ if (vivid_is_svid_out(dev)) {
+ switch (field) {
+ case V4L2_FIELD_INTERLACED_TB:
+ case V4L2_FIELD_INTERLACED_BT:
+ case V4L2_FIELD_SEQ_TB:
+ case V4L2_FIELD_SEQ_BT:
+ case V4L2_FIELD_ALTERNATE:
+ return field;
+ case V4L2_FIELD_INTERLACED:
+ default:
+ return V4L2_FIELD_INTERLACED;
+ }
+ }
+ if (vivid_is_hdmi_out(dev))
+ return dev->dv_timings_out.bt.interlaced ? V4L2_FIELD_ALTERNATE :
+ V4L2_FIELD_NONE;
+ return V4L2_FIELD_NONE;
+}
+
+static enum tpg_pixel_aspect vivid_get_pixel_aspect(const struct vivid_dev *dev)
+{
+ if (vivid_is_svid_out(dev))
+ return (dev->std_out & V4L2_STD_525_60) ?
+ TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+ if (vivid_is_hdmi_out(dev) &&
+ dev->sink_rect.width == 720 && dev->sink_rect.height <= 576)
+ return dev->sink_rect.height == 480 ?
+ TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+ return TPG_PIXEL_ASPECT_SQUARE;
+}
+
+int vivid_g_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+ const struct vivid_fmt *fmt = dev->fmt_out;
+ unsigned p;
+
+ mp->width = dev->fmt_out_rect.width;
+ mp->height = dev->fmt_out_rect.height;
+ mp->field = dev->field_out;
+ mp->pixelformat = fmt->fourcc;
+ mp->colorspace = dev->colorspace_out;
+ mp->xfer_func = dev->xfer_func_out;
+ mp->ycbcr_enc = dev->ycbcr_enc_out;
+ mp->quantization = dev->quantization_out;
+ mp->num_planes = fmt->buffers;
+ for (p = 0; p < mp->num_planes; p++) {
+ mp->plane_fmt[p].bytesperline = dev->bytesperline_out[p];
+ mp->plane_fmt[p].sizeimage =
+ mp->plane_fmt[p].bytesperline * mp->height +
+ fmt->data_offset[p];
+ }
+ for (p = fmt->buffers; p < fmt->planes; p++) {
+ unsigned stride = dev->bytesperline_out[p];
+
+ mp->plane_fmt[0].sizeimage +=
+ (stride * mp->height) / fmt->vdownsampling[p];
+ }
+ return 0;
+}
+
+int vivid_try_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
+ struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+ struct v4l2_plane_pix_format *pfmt = mp->plane_fmt;
+ const struct vivid_fmt *fmt;
+ unsigned bytesperline, max_bpl;
+ unsigned factor = 1;
+ unsigned w, h;
+ unsigned p;
+
+ fmt = vivid_get_format(dev, mp->pixelformat);
+ if (!fmt) {
+ dprintk(dev, 1, "Fourcc format (0x%08x) unknown.\n",
+ mp->pixelformat);
+ mp->pixelformat = V4L2_PIX_FMT_YUYV;
+ fmt = vivid_get_format(dev, mp->pixelformat);
+ }
+
+ mp->field = vivid_field_out(dev, mp->field);
+ if (vivid_is_svid_out(dev)) {
+ w = 720;
+ h = (dev->std_out & V4L2_STD_525_60) ? 480 : 576;
+ } else {
+ w = dev->sink_rect.width;
+ h = dev->sink_rect.height;
+ }
+ if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+ factor = 2;
+ if (!dev->has_scaler_out && !dev->has_crop_out && !dev->has_compose_out) {
+ mp->width = w;
+ mp->height = h / factor;
+ } else {
+ struct v4l2_rect r = { 0, 0, mp->width, mp->height * factor };
+
+ v4l2_rect_set_min_size(&r, &vivid_min_rect);
+ v4l2_rect_set_max_size(&r, &vivid_max_rect);
+ if (dev->has_scaler_out && !dev->has_crop_out) {
+ struct v4l2_rect max_r = { 0, 0, MAX_ZOOM * w, MAX_ZOOM * h };
+
+ v4l2_rect_set_max_size(&r, &max_r);
+ } else if (!dev->has_scaler_out && dev->has_compose_out && !dev->has_crop_out) {
+ v4l2_rect_set_max_size(&r, &dev->sink_rect);
+ } else if (!dev->has_scaler_out && !dev->has_compose_out) {
+ v4l2_rect_set_min_size(&r, &dev->sink_rect);
+ }
+ mp->width = r.width;
+ mp->height = r.height / factor;
+ }
+
+ /* This driver supports custom bytesperline values */
+
+ mp->num_planes = fmt->buffers;
+ for (p = 0; p < fmt->buffers; p++) {
+ /* Calculate the minimum supported bytesperline value */
+ bytesperline = (mp->width * fmt->bit_depth[p]) >> 3;
+ /* Calculate the maximum supported bytesperline value */
+ max_bpl = (MAX_ZOOM * MAX_WIDTH * fmt->bit_depth[p]) >> 3;
+
+ if (pfmt[p].bytesperline > max_bpl)
+ pfmt[p].bytesperline = max_bpl;
+ if (pfmt[p].bytesperline < bytesperline)
+ pfmt[p].bytesperline = bytesperline;
+
+ pfmt[p].sizeimage = (pfmt[p].bytesperline * mp->height) /
+ fmt->vdownsampling[p] + fmt->data_offset[p];
+
+ memset(pfmt[p].reserved, 0, sizeof(pfmt[p].reserved));
+ }
+ for (p = fmt->buffers; p < fmt->planes; p++)
+ pfmt[0].sizeimage += (pfmt[0].bytesperline * mp->height *
+ (fmt->bit_depth[p] / fmt->vdownsampling[p])) /
+ (fmt->bit_depth[0] / fmt->vdownsampling[0]);
+
+ mp->xfer_func = V4L2_XFER_FUNC_DEFAULT;
+ mp->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
+ mp->quantization = V4L2_QUANTIZATION_DEFAULT;
+ if (vivid_is_svid_out(dev)) {
+ mp->colorspace = V4L2_COLORSPACE_SMPTE170M;
+ } else if (dev->dvi_d_out || !(bt->flags & V4L2_DV_FL_IS_CE_VIDEO)) {
+ mp->colorspace = V4L2_COLORSPACE_SRGB;
+ if (dev->dvi_d_out)
+ mp->quantization = V4L2_QUANTIZATION_LIM_RANGE;
+ } else if (bt->width == 720 && bt->height <= 576) {
+ mp->colorspace = V4L2_COLORSPACE_SMPTE170M;
+ } else if (mp->colorspace != V4L2_COLORSPACE_SMPTE170M &&
+ mp->colorspace != V4L2_COLORSPACE_REC709 &&
+ mp->colorspace != V4L2_COLORSPACE_OPRGB &&
+ mp->colorspace != V4L2_COLORSPACE_BT2020 &&
+ mp->colorspace != V4L2_COLORSPACE_SRGB) {
+ mp->colorspace = V4L2_COLORSPACE_REC709;
+ }
+ memset(mp->reserved, 0, sizeof(mp->reserved));
+ return 0;
+}
+
+int vivid_s_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_rect *crop = &dev->crop_out;
+ struct v4l2_rect *compose = &dev->compose_out;
+ struct vb2_queue *q = &dev->vb_vid_out_q;
+ int ret = vivid_try_fmt_vid_out(file, priv, f);
+ unsigned factor = 1;
+ unsigned p;
+
+ if (ret < 0)
+ return ret;
+
+ if (vb2_is_busy(q) &&
+ (vivid_is_svid_out(dev) ||
+ mp->width != dev->fmt_out_rect.width ||
+ mp->height != dev->fmt_out_rect.height ||
+ mp->pixelformat != dev->fmt_out->fourcc ||
+ mp->field != dev->field_out)) {
+ dprintk(dev, 1, "%s device busy\n", __func__);
+ return -EBUSY;
+ }
+
+ /*
+ * Allow for changing the colorspace on the fly. Useful for testing
+ * purposes, and it is something that HDMI transmitters are able
+ * to do.
+ */
+ if (vb2_is_busy(q))
+ goto set_colorspace;
+
+ dev->fmt_out = vivid_get_format(dev, mp->pixelformat);
+ if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+ factor = 2;
+
+ if (dev->has_scaler_out || dev->has_crop_out || dev->has_compose_out) {
+ struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+ if (dev->has_scaler_out) {
+ if (dev->has_crop_out)
+ v4l2_rect_map_inside(crop, &r);
+ else
+ *crop = r;
+ if (dev->has_compose_out && !dev->has_crop_out) {
+ struct v4l2_rect min_r = {
+ 0, 0,
+ r.width / MAX_ZOOM,
+ factor * r.height / MAX_ZOOM
+ };
+ struct v4l2_rect max_r = {
+ 0, 0,
+ r.width * MAX_ZOOM,
+ factor * r.height * MAX_ZOOM
+ };
+
+ v4l2_rect_set_min_size(compose, &min_r);
+ v4l2_rect_set_max_size(compose, &max_r);
+ v4l2_rect_map_inside(compose, &dev->compose_bounds_out);
+ } else if (dev->has_compose_out) {
+ struct v4l2_rect min_r = {
+ 0, 0,
+ crop->width / MAX_ZOOM,
+ factor * crop->height / MAX_ZOOM
+ };
+ struct v4l2_rect max_r = {
+ 0, 0,
+ crop->width * MAX_ZOOM,
+ factor * crop->height * MAX_ZOOM
+ };
+
+ v4l2_rect_set_min_size(compose, &min_r);
+ v4l2_rect_set_max_size(compose, &max_r);
+ v4l2_rect_map_inside(compose, &dev->compose_bounds_out);
+ }
+ } else if (dev->has_compose_out && !dev->has_crop_out) {
+ v4l2_rect_set_size_to(crop, &r);
+ r.height *= factor;
+ v4l2_rect_set_size_to(compose, &r);
+ v4l2_rect_map_inside(compose, &dev->compose_bounds_out);
+ } else if (!dev->has_compose_out) {
+ v4l2_rect_map_inside(crop, &r);
+ r.height /= factor;
+ v4l2_rect_set_size_to(compose, &r);
+ } else {
+ r.height *= factor;
+ v4l2_rect_set_max_size(compose, &r);
+ v4l2_rect_map_inside(compose, &dev->compose_bounds_out);
+ crop->top *= factor;
+ crop->height *= factor;
+ v4l2_rect_set_size_to(crop, compose);
+ v4l2_rect_map_inside(crop, &r);
+ crop->top /= factor;
+ crop->height /= factor;
+ }
+ } else {
+ struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+ v4l2_rect_set_size_to(crop, &r);
+ r.height /= factor;
+ v4l2_rect_set_size_to(compose, &r);
+ }
+
+ dev->fmt_out_rect.width = mp->width;
+ dev->fmt_out_rect.height = mp->height;
+ for (p = 0; p < mp->num_planes; p++)
+ dev->bytesperline_out[p] = mp->plane_fmt[p].bytesperline;
+ for (p = dev->fmt_out->buffers; p < dev->fmt_out->planes; p++)
+ dev->bytesperline_out[p] =
+ (dev->bytesperline_out[0] * dev->fmt_out->bit_depth[p]) /
+ dev->fmt_out->bit_depth[0];
+ dev->field_out = mp->field;
+ if (vivid_is_svid_out(dev))
+ dev->tv_field_out = mp->field;
+
+set_colorspace:
+ dev->colorspace_out = mp->colorspace;
+ dev->xfer_func_out = mp->xfer_func;
+ dev->ycbcr_enc_out = mp->ycbcr_enc;
+ dev->quantization_out = mp->quantization;
+ if (dev->loop_video) {
+ vivid_send_source_change(dev, SVID);
+ vivid_send_source_change(dev, HDMI);
+ }
+ return 0;
+}
+
+int vidioc_g_fmt_vid_out_mplane(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!dev->multiplanar)
+ return -ENOTTY;
+ return vivid_g_fmt_vid_out(file, priv, f);
+}
+
+int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!dev->multiplanar)
+ return -ENOTTY;
+ return vivid_try_fmt_vid_out(file, priv, f);
+}
+
+int vidioc_s_fmt_vid_out_mplane(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!dev->multiplanar)
+ return -ENOTTY;
+ return vivid_s_fmt_vid_out(file, priv, f);
+}
+
+int vidioc_g_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+ return fmt_sp2mp_func(file, priv, f, vivid_g_fmt_vid_out);
+}
+
+int vidioc_try_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+ return fmt_sp2mp_func(file, priv, f, vivid_try_fmt_vid_out);
+}
+
+int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (dev->multiplanar)
+ return -ENOTTY;
+ return fmt_sp2mp_func(file, priv, f, vivid_s_fmt_vid_out);
+}
+
+int vivid_vid_out_g_selection(struct file *file, void *priv,
+ struct v4l2_selection *sel)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!dev->has_crop_out && !dev->has_compose_out)
+ return -ENOTTY;
+ if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+ return -EINVAL;
+
+ sel->r.left = sel->r.top = 0;
+ switch (sel->target) {
+ case V4L2_SEL_TGT_CROP:
+ if (!dev->has_crop_out)
+ return -EINVAL;
+ sel->r = dev->crop_out;
+ break;
+ case V4L2_SEL_TGT_CROP_DEFAULT:
+ if (!dev->has_crop_out)
+ return -EINVAL;
+ sel->r = dev->fmt_out_rect;
+ break;
+ case V4L2_SEL_TGT_CROP_BOUNDS:
+ if (!dev->has_crop_out)
+ return -EINVAL;
+ sel->r = vivid_max_rect;
+ break;
+ case V4L2_SEL_TGT_COMPOSE:
+ if (!dev->has_compose_out)
+ return -EINVAL;
+ sel->r = dev->compose_out;
+ break;
+ case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+ case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+ if (!dev->has_compose_out)
+ return -EINVAL;
+ sel->r = dev->sink_rect;
+ break;
+ default:
+ return -EINVAL;
+ }
+ return 0;
+}
+
+int vivid_vid_out_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ struct v4l2_rect *crop = &dev->crop_out;
+ struct v4l2_rect *compose = &dev->compose_out;
+ unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_out) ? 2 : 1;
+ int ret;
+
+ if (!dev->has_crop_out && !dev->has_compose_out)
+ return -ENOTTY;
+ if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+ return -EINVAL;
+
+ switch (s->target) {
+ case V4L2_SEL_TGT_CROP:
+ if (!dev->has_crop_out)
+ return -EINVAL;
+ ret = vivid_vid_adjust_sel(s->flags, &s->r);
+ if (ret)
+ return ret;
+ v4l2_rect_set_min_size(&s->r, &vivid_min_rect);
+ v4l2_rect_set_max_size(&s->r, &dev->fmt_out_rect);
+ if (dev->has_scaler_out) {
+ struct v4l2_rect max_rect = {
+ 0, 0,
+ dev->sink_rect.width * MAX_ZOOM,
+ (dev->sink_rect.height / factor) * MAX_ZOOM
+ };
+
+ v4l2_rect_set_max_size(&s->r, &max_rect);
+ if (dev->has_compose_out) {
+ struct v4l2_rect min_rect = {
+ 0, 0,
+ s->r.width / MAX_ZOOM,
+ (s->r.height * factor) / MAX_ZOOM
+ };
+ struct v4l2_rect max_rect = {
+ 0, 0,
+ s->r.width * MAX_ZOOM,
+ (s->r.height * factor) * MAX_ZOOM
+ };
+
+ v4l2_rect_set_min_size(compose, &min_rect);
+ v4l2_rect_set_max_size(compose, &max_rect);
+ v4l2_rect_map_inside(compose, &dev->compose_bounds_out);
+ }
+ } else if (dev->has_compose_out) {
+ s->r.top *= factor;
+ s->r.height *= factor;
+ v4l2_rect_set_max_size(&s->r, &dev->sink_rect);
+ v4l2_rect_set_size_to(compose, &s->r);
+ v4l2_rect_map_inside(compose, &dev->compose_bounds_out);
+ s->r.top /= factor;
+ s->r.height /= factor;
+ } else {
+ v4l2_rect_set_size_to(&s->r, &dev->sink_rect);
+ s->r.height /= factor;
+ }
+ v4l2_rect_map_inside(&s->r, &dev->fmt_out_rect);
+ *crop = s->r;
+ break;
+ case V4L2_SEL_TGT_COMPOSE:
+ if (!dev->has_compose_out)
+ return -EINVAL;
+ ret = vivid_vid_adjust_sel(s->flags, &s->r);
+ if (ret)
+ return ret;
+ v4l2_rect_set_min_size(&s->r, &vivid_min_rect);
+ v4l2_rect_set_max_size(&s->r, &dev->sink_rect);
+ v4l2_rect_map_inside(&s->r, &dev->compose_bounds_out);
+ s->r.top /= factor;
+ s->r.height /= factor;
+ if (dev->has_scaler_out) {
+ struct v4l2_rect fmt = dev->fmt_out_rect;
+ struct v4l2_rect max_rect = {
+ 0, 0,
+ s->r.width * MAX_ZOOM,
+ s->r.height * MAX_ZOOM
+ };
+ struct v4l2_rect min_rect = {
+ 0, 0,
+ s->r.width / MAX_ZOOM,
+ s->r.height / MAX_ZOOM
+ };
+
+ v4l2_rect_set_min_size(&fmt, &min_rect);
+ if (!dev->has_crop_out)
+ v4l2_rect_set_max_size(&fmt, &max_rect);
+ if (!v4l2_rect_same_size(&dev->fmt_out_rect, &fmt) &&
+ vb2_is_busy(&dev->vb_vid_out_q))
+ return -EBUSY;
+ if (dev->has_crop_out) {
+ v4l2_rect_set_min_size(crop, &min_rect);
+ v4l2_rect_set_max_size(crop, &max_rect);
+ }
+ dev->fmt_out_rect = fmt;
+ } else if (dev->has_crop_out) {
+ struct v4l2_rect fmt = dev->fmt_out_rect;
+
+ v4l2_rect_set_min_size(&fmt, &s->r);
+ if (!v4l2_rect_same_size(&dev->fmt_out_rect, &fmt) &&
+ vb2_is_busy(&dev->vb_vid_out_q))
+ return -EBUSY;
+ dev->fmt_out_rect = fmt;
+ v4l2_rect_set_size_to(crop, &s->r);
+ v4l2_rect_map_inside(crop, &dev->fmt_out_rect);
+ } else {
+ if (!v4l2_rect_same_size(&s->r, &dev->fmt_out_rect) &&
+ vb2_is_busy(&dev->vb_vid_out_q))
+ return -EBUSY;
+ v4l2_rect_set_size_to(&dev->fmt_out_rect, &s->r);
+ v4l2_rect_set_size_to(crop, &s->r);
+ crop->height /= factor;
+ v4l2_rect_map_inside(crop, &dev->fmt_out_rect);
+ }
+ s->r.top *= factor;
+ s->r.height *= factor;
+ if (dev->bitmap_out && (compose->width != s->r.width ||
+ compose->height != s->r.height)) {
+ vfree(dev->bitmap_out);
+ dev->bitmap_out = NULL;
+ }
+ *compose = s->r;
+ break;
+ default:
+ return -EINVAL;
+ }
+
+ return 0;
+}
+
+int vivid_vid_out_g_pixelaspect(struct file *file, void *priv,
+ int type, struct v4l2_fract *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+ return -EINVAL;
+
+ switch (vivid_get_pixel_aspect(dev)) {
+ case TPG_PIXEL_ASPECT_NTSC:
+ f->numerator = 11;
+ f->denominator = 10;
+ break;
+ case TPG_PIXEL_ASPECT_PAL:
+ f->numerator = 54;
+ f->denominator = 59;
+ break;
+ default:
+ break;
+ }
+ return 0;
+}
+
+int vidioc_g_fmt_vid_out_overlay(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct v4l2_rect *compose = &dev->compose_out;
+ struct v4l2_window *win = &f->fmt.win;
+ unsigned clipcount = win->clipcount;
+
+ if (!dev->has_fb)
+ return -EINVAL;
+ win->w.top = dev->overlay_out_top;
+ win->w.left = dev->overlay_out_left;
+ win->w.width = compose->width;
+ win->w.height = compose->height;
+ win->clipcount = dev->clipcount_out;
+ win->field = V4L2_FIELD_ANY;
+ win->chromakey = dev->chromakey_out;
+ win->global_alpha = dev->global_alpha_out;
+ if (clipcount > dev->clipcount_out)
+ clipcount = dev->clipcount_out;
+ if (dev->bitmap_out == NULL)
+ win->bitmap = NULL;
+ else if (win->bitmap) {
+ if (copy_to_user(win->bitmap, dev->bitmap_out,
+ ((dev->compose_out.width + 7) / 8) * dev->compose_out.height))
+ return -EFAULT;
+ }
+ if (clipcount && win->clips) {
+ if (copy_to_user(win->clips, dev->clips_out,
+ clipcount * sizeof(dev->clips_out[0])))
+ return -EFAULT;
+ }
+ return 0;
+}
+
+int vidioc_try_fmt_vid_out_overlay(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct v4l2_rect *compose = &dev->compose_out;
+ struct v4l2_window *win = &f->fmt.win;
+ int i, j;
+
+ if (!dev->has_fb)
+ return -EINVAL;
+ win->w.left = clamp_t(int, win->w.left,
+ -dev->display_width, dev->display_width);
+ win->w.top = clamp_t(int, win->w.top,
+ -dev->display_height, dev->display_height);
+ win->w.width = compose->width;
+ win->w.height = compose->height;
+ /*
+ * It makes no sense for an OSD to overlay only top or bottom fields,
+ * so always set this to ANY.
+ */
+ win->field = V4L2_FIELD_ANY;
+ if (win->clipcount && !win->clips)
+ win->clipcount = 0;
+ if (win->clipcount > MAX_CLIPS)
+ win->clipcount = MAX_CLIPS;
+ if (win->clipcount) {
+ if (copy_from_user(dev->try_clips_out, win->clips,
+ win->clipcount * sizeof(dev->clips_out[0])))
+ return -EFAULT;
+ for (i = 0; i < win->clipcount; i++) {
+ struct v4l2_rect *r = &dev->try_clips_out[i].c;
+
+ r->top = clamp_t(s32, r->top, 0, dev->display_height - 1);
+ r->height = clamp_t(s32, r->height, 1, dev->display_height - r->top);
+ r->left = clamp_t(u32, r->left, 0, dev->display_width - 1);
+ r->width = clamp_t(u32, r->width, 1, dev->display_width - r->left);
+ }
+ /*
+ * Yeah, so sue me, it's an O(n^2) algorithm. But n is a small
+ * number and it's typically a one-time deal.
+ */
+ for (i = 0; i < win->clipcount - 1; i++) {
+ struct v4l2_rect *r1 = &dev->try_clips_out[i].c;
+
+ for (j = i + 1; j < win->clipcount; j++) {
+ struct v4l2_rect *r2 = &dev->try_clips_out[j].c;
+
+ if (v4l2_rect_overlap(r1, r2))
+ return -EINVAL;
+ }
+ }
+ if (copy_to_user(win->clips, dev->try_clips_out,
+ win->clipcount * sizeof(dev->clips_out[0])))
+ return -EFAULT;
+ }
+ return 0;
+}
+
+int vidioc_s_fmt_vid_out_overlay(struct file *file, void *priv,
+ struct v4l2_format *f)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const struct v4l2_rect *compose = &dev->compose_out;
+ struct v4l2_window *win = &f->fmt.win;
+ int ret = vidioc_try_fmt_vid_out_overlay(file, priv, f);
+ unsigned bitmap_size = ((compose->width + 7) / 8) * compose->height;
+ unsigned clips_size = win->clipcount * sizeof(dev->clips_out[0]);
+ void *new_bitmap = NULL;
+
+ if (ret)
+ return ret;
+
+ if (win->bitmap) {
+ new_bitmap = vzalloc(bitmap_size);
+
+ if (!new_bitmap)
+ return -ENOMEM;
+ if (copy_from_user(new_bitmap, win->bitmap, bitmap_size)) {
+ vfree(new_bitmap);
+ return -EFAULT;
+ }
+ }
+
+ dev->overlay_out_top = win->w.top;
+ dev->overlay_out_left = win->w.left;
+ vfree(dev->bitmap_out);
+ dev->bitmap_out = new_bitmap;
+ dev->clipcount_out = win->clipcount;
+ if (dev->clipcount_out)
+ memcpy(dev->clips_out, dev->try_clips_out, clips_size);
+ dev->chromakey_out = win->chromakey;
+ dev->global_alpha_out = win->global_alpha;
+ return ret;
+}
+
+int vivid_vid_out_overlay(struct file *file, void *fh, unsigned i)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (i && !dev->fmt_out->can_do_overlay) {
+ dprintk(dev, 1, "unsupported output format for output overlay\n");
+ return -EINVAL;
+ }
+
+ dev->overlay_out_enabled = i;
+ return 0;
+}
+
+int vivid_vid_out_g_fbuf(struct file *file, void *fh,
+ struct v4l2_framebuffer *a)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
+ V4L2_FBUF_CAP_BITMAP_CLIPPING |
+ V4L2_FBUF_CAP_LIST_CLIPPING |
+ V4L2_FBUF_CAP_CHROMAKEY |
+ V4L2_FBUF_CAP_SRC_CHROMAKEY |
+ V4L2_FBUF_CAP_GLOBAL_ALPHA |
+ V4L2_FBUF_CAP_LOCAL_ALPHA |
+ V4L2_FBUF_CAP_LOCAL_INV_ALPHA;
+ a->flags = V4L2_FBUF_FLAG_OVERLAY | dev->fbuf_out_flags;
+ a->base = (void *)dev->video_pbase;
+ a->fmt.width = dev->display_width;
+ a->fmt.height = dev->display_height;
+ if (dev->fb_defined.green.length == 5)
+ a->fmt.pixelformat = V4L2_PIX_FMT_ARGB555;
+ else
+ a->fmt.pixelformat = V4L2_PIX_FMT_RGB565;
+ a->fmt.bytesperline = dev->display_byte_stride;
+ a->fmt.sizeimage = a->fmt.height * a->fmt.bytesperline;
+ a->fmt.field = V4L2_FIELD_NONE;
+ a->fmt.colorspace = V4L2_COLORSPACE_SRGB;
+ a->fmt.priv = 0;
+ return 0;
+}
+
+int vivid_vid_out_s_fbuf(struct file *file, void *fh,
+ const struct v4l2_framebuffer *a)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ const unsigned chroma_flags = V4L2_FBUF_FLAG_CHROMAKEY |
+ V4L2_FBUF_FLAG_SRC_CHROMAKEY;
+ const unsigned alpha_flags = V4L2_FBUF_FLAG_GLOBAL_ALPHA |
+ V4L2_FBUF_FLAG_LOCAL_ALPHA |
+ V4L2_FBUF_FLAG_LOCAL_INV_ALPHA;
+
+
+ if ((a->flags & chroma_flags) == chroma_flags)
+ return -EINVAL;
+ switch (a->flags & alpha_flags) {
+ case 0:
+ case V4L2_FBUF_FLAG_GLOBAL_ALPHA:
+ case V4L2_FBUF_FLAG_LOCAL_ALPHA:
+ case V4L2_FBUF_FLAG_LOCAL_INV_ALPHA:
+ break;
+ default:
+ return -EINVAL;
+ }
+ dev->fbuf_out_flags &= ~(chroma_flags | alpha_flags);
+ dev->fbuf_out_flags |= a->flags & (chroma_flags | alpha_flags);
+ return 0;
+}
+
+static const struct v4l2_audioout vivid_audio_outputs[] = {
+ { 0, "Line-Out 1" },
+ { 1, "Line-Out 2" },
+};
+
+int vidioc_enum_output(struct file *file, void *priv,
+ struct v4l2_output *out)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (out->index >= dev->num_outputs)
+ return -EINVAL;
+
+ out->type = V4L2_OUTPUT_TYPE_ANALOG;
+ switch (dev->output_type[out->index]) {
+ case SVID:
+ snprintf(out->name, sizeof(out->name), "S-Video %u",
+ dev->output_name_counter[out->index]);
+ out->std = V4L2_STD_ALL;
+ if (dev->has_audio_outputs)
+ out->audioset = (1 << ARRAY_SIZE(vivid_audio_outputs)) - 1;
+ out->capabilities = V4L2_OUT_CAP_STD;
+ break;
+ case HDMI:
+ snprintf(out->name, sizeof(out->name), "HDMI %u",
+ dev->output_name_counter[out->index]);
+ out->capabilities = V4L2_OUT_CAP_DV_TIMINGS;
+ break;
+ }
+ return 0;
+}
+
+int vidioc_g_output(struct file *file, void *priv, unsigned *o)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ *o = dev->output;
+ return 0;
+}
+
+int vidioc_s_output(struct file *file, void *priv, unsigned o)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (o >= dev->num_outputs)
+ return -EINVAL;
+
+ if (o == dev->output)
+ return 0;
+
+ if (vb2_is_busy(&dev->vb_vid_out_q) ||
+ vb2_is_busy(&dev->vb_vbi_out_q) ||
+ vb2_is_busy(&dev->vb_meta_out_q))
+ return -EBUSY;
+
+ dev->output = o;
+ dev->tv_audio_output = 0;
+ if (dev->output_type[o] == SVID)
+ dev->vid_out_dev.tvnorms = V4L2_STD_ALL;
+ else
+ dev->vid_out_dev.tvnorms = 0;
+
+ dev->vbi_out_dev.tvnorms = dev->vid_out_dev.tvnorms;
+ dev->meta_out_dev.tvnorms = dev->vid_out_dev.tvnorms;
+ vivid_update_format_out(dev);
+
+ v4l2_ctrl_activate(dev->ctrl_display_present, vivid_is_hdmi_out(dev));
+ if (vivid_is_hdmi_out(dev))
+ v4l2_ctrl_s_ctrl(dev->ctrl_display_present,
+ dev->display_present[dev->output]);
+
+ return 0;
+}
+
+int vidioc_enumaudout(struct file *file, void *fh, struct v4l2_audioout *vout)
+{
+ if (vout->index >= ARRAY_SIZE(vivid_audio_outputs))
+ return -EINVAL;
+ *vout = vivid_audio_outputs[vout->index];
+ return 0;
+}
+
+int vidioc_g_audout(struct file *file, void *fh, struct v4l2_audioout *vout)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_svid_out(dev))
+ return -EINVAL;
+ *vout = vivid_audio_outputs[dev->tv_audio_output];
+ return 0;
+}
+
+int vidioc_s_audout(struct file *file, void *fh, const struct v4l2_audioout *vout)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_svid_out(dev))
+ return -EINVAL;
+ if (vout->index >= ARRAY_SIZE(vivid_audio_outputs))
+ return -EINVAL;
+ dev->tv_audio_output = vout->index;
+ return 0;
+}
+
+int vivid_vid_out_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (!vivid_is_svid_out(dev))
+ return -ENODATA;
+ if (dev->std_out == id)
+ return 0;
+ if (vb2_is_busy(&dev->vb_vid_out_q) || vb2_is_busy(&dev->vb_vbi_out_q))
+ return -EBUSY;
+ dev->std_out = id;
+ vivid_update_format_out(dev);
+ return 0;
+}
+
+static bool valid_cvt_gtf_timings(struct v4l2_dv_timings *timings)
+{
+ struct v4l2_bt_timings *bt = &timings->bt;
+
+ if ((bt->standards & (V4L2_DV_BT_STD_CVT | V4L2_DV_BT_STD_GTF)) &&
+ v4l2_valid_dv_timings(timings, &vivid_dv_timings_cap, NULL, NULL))
+ return true;
+
+ return false;
+}
+
+int vivid_vid_out_s_dv_timings(struct file *file, void *_fh,
+ struct v4l2_dv_timings *timings)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+ if (!vivid_is_hdmi_out(dev))
+ return -ENODATA;
+ if (!v4l2_find_dv_timings_cap(timings, &vivid_dv_timings_cap,
+ 0, NULL, NULL) &&
+ !valid_cvt_gtf_timings(timings))
+ return -EINVAL;
+ if (v4l2_match_dv_timings(timings, &dev->dv_timings_out, 0, true))
+ return 0;
+ if (vb2_is_busy(&dev->vb_vid_out_q))
+ return -EBUSY;
+ dev->dv_timings_out = *timings;
+ vivid_update_format_out(dev);
+ return 0;
+}
+
+int vivid_vid_out_g_parm(struct file *file, void *priv,
+ struct v4l2_streamparm *parm)
+{
+ struct vivid_dev *dev = video_drvdata(file);
+
+ if (parm->type != (dev->multiplanar ?
+ V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+ V4L2_BUF_TYPE_VIDEO_OUTPUT))
+ return -EINVAL;
+
+ parm->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
+ parm->parm.output.timeperframe = dev->timeperframe_vid_out;
+ parm->parm.output.writebuffers = 1;
+
+ return 0;
+}
+
+int vidioc_subscribe_event(struct v4l2_fh *fh,
+ const struct v4l2_event_subscription *sub)
+{
+ switch (sub->type) {
+ case V4L2_EVENT_SOURCE_CHANGE:
+ if (fh->vdev->vfl_dir == VFL_DIR_RX)
+ return v4l2_src_change_event_subscribe(fh, sub);
+ break;
+ default:
+ return v4l2_ctrl_subscribe_event(fh, sub);
+ }
+ return -EINVAL;
+}
diff --git a/drivers/media/test-drivers/vivid/vivid-vid-out.h b/drivers/media/test-drivers/vivid/vivid-vid-out.h
new file mode 100644
index 0000000..8d56314
--- /dev/null
+++ b/drivers/media/test-drivers/vivid/vivid-vid-out.h
@@ -0,0 +1,44 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * vivid-vid-out.h - video output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ */
+
+#ifndef _VIVID_VID_OUT_H_
+#define _VIVID_VID_OUT_H_
+
+extern const struct vb2_ops vivid_vid_out_qops;
+
+void vivid_update_format_out(struct vivid_dev *dev);
+
+int vivid_g_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_try_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_s_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_out_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_out_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_out_g_selection(struct file *file, void *priv, struct v4l2_selection *sel);
+int vivid_vid_out_s_selection(struct file *file, void *fh, struct v4l2_selection *s);
+int vivid_vid_out_g_pixelaspect(struct file *file, void *priv, int type, struct v4l2_fract *f);
+int vidioc_enum_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_out_overlay(struct file *file, void *fh, unsigned i);
+int vivid_vid_out_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *a);
+int vivid_vid_out_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *a);
+int vidioc_enum_output(struct file *file, void *priv, struct v4l2_output *out);
+int vidioc_g_output(struct file *file, void *priv, unsigned *i);
+int vidioc_s_output(struct file *file, void *priv, unsigned i);
+int vidioc_enumaudout(struct file *file, void *fh, struct v4l2_audioout *vout);
+int vidioc_g_audout(struct file *file, void *fh, struct v4l2_audioout *vout);
+int vidioc_s_audout(struct file *file, void *fh, const struct v4l2_audioout *vout);
+int vivid_vid_out_s_std(struct file *file, void *priv, v4l2_std_id id);
+int vivid_vid_out_s_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vivid_vid_out_g_parm(struct file *file, void *priv, struct v4l2_streamparm *parm);
+
+#endif