[media] pci: Add tw5864 driver

Support for boards based on Techwell TW5864 chip which provides
multichannel video & audio grabbing and encoding (H.264, MJPEG,
ADPCM G.726).

This submission implements only H.264 encoding of all channels at D1
resolution.

Thanks to Mark Thompson <sw@jkqxz.net> for help, and for contribution of
H.264 startcode emulation prevention code.
Signed-off-by: NAndrey Utkin <andrey.utkin@corp.bluecherry.net>
Signed-off-by: NHans Verkuil <hans.verkuil@cisco.com>
Signed-off-by: NMauro Carvalho Chehab <mchehab@s-opensource.com>

MAINTAINERS

@@ -11839,6 +11839,14 @@ T:	git git://linuxtv.org/media_tree.git
 S:	Odd fixes
 F:	drivers/media/usb/tm6000/
 
+TW5864 VIDEO4LINUX DRIVER
+M:	Bluecherry Maintainers <maintainers@bluecherrydvr.com>
+M:	Andrey Utkin <andrey.utkin@corp.bluecherry.net>
+M:	Andrey Utkin <andrey_utkin@fastmail.com>
+L:	linux-media@vger.kernel.org
+S:	Supported
+F:	drivers/media/pci/tw5864/
+
 TW68 VIDEO4LINUX DRIVER
 M:	Hans Verkuil <hverkuil@xs4all.nl>
 L:	linux-media@vger.kernel.org

drivers/media/pci/Kconfig

@@ -13,6 +13,7 @@ if MEDIA_CAMERA_SUPPORT
 source "drivers/media/pci/meye/Kconfig"
 source "drivers/media/pci/solo6x10/Kconfig"
 source "drivers/media/pci/sta2x11/Kconfig"
+source "drivers/media/pci/tw5864/Kconfig"
 source "drivers/media/pci/tw68/Kconfig"
 source "drivers/media/pci/tw686x/Kconfig"
 source "drivers/media/pci/zoran/Kconfig"

drivers/media/pci/Makefile

@@ -31,3 +31,4 @@ obj-$(CONFIG_VIDEO_MEYE) += meye/
 obj-$(CONFIG_STA2X11_VIP) += sta2x11/
 obj-$(CONFIG_VIDEO_SOLO6X10) += solo6x10/
 obj-$(CONFIG_VIDEO_COBALT) += cobalt/
+obj-$(CONFIG_VIDEO_TW5864) += tw5864/

drivers/media/pci/tw5864/Kconfig

+config VIDEO_TW5864
+	tristate "Techwell TW5864 video/audio grabber and encoder"
+	depends on VIDEO_DEV && PCI && VIDEO_V4L2
+	select VIDEOBUF2_DMA_CONTIG
+	---help---
+	  Support for boards based on Techwell TW5864 chip which provides
+	  multichannel video & audio grabbing and encoding (H.264, MJPEG,
+	  ADPCM G.726).
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw5864.

drivers/media/pci/tw5864/Makefile

+tw5864-objs := tw5864-core.o tw5864-video.o tw5864-h264.o tw5864-util.o
+
+obj-$(CONFIG_VIDEO_TW5864) += tw5864.o

drivers/media/pci/tw5864/tw5864-core.c

+/*
+ *  TW5864 driver - core functions
+ *
+ *  Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.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.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/kmod.h>
+#include <linux/sound.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/pm.h>
+#include <linux/pci_ids.h>
+#include <linux/jiffies.h>
+#include <asm/dma.h>
+#include <media/v4l2-dev.h>
+
+#include "tw5864.h"
+#include "tw5864-reg.h"
+
+MODULE_DESCRIPTION("V4L2 driver module for tw5864-based multimedia capture & encoding devices");
+MODULE_AUTHOR("Bluecherry Maintainers <maintainers@bluecherrydvr.com>");
+MODULE_AUTHOR("Andrey Utkin <andrey.utkin@corp.bluecherry.net>");
+MODULE_LICENSE("GPL");
+
+/*
+ * BEWARE OF KNOWN ISSUES WITH VIDEO QUALITY
+ *
+ * This driver was developed by Bluecherry LLC by deducing behaviour of
+ * original manufacturer's driver, from both source code and execution traces.
+ * It is known that there are some artifacts on output video with this driver:
+ *  - on all known hardware samples: random pixels of wrong color (mostly
+ *    white, red or blue) appearing and disappearing on sequences of P-frames;
+ *  - on some hardware samples (known with H.264 core version e006:2800):
+ *    total madness on P-frames: blocks of wrong luminance; blocks of wrong
+ *    colors "creeping" across the picture.
+ * There is a workaround for both issues: avoid P-frames by setting GOP size
+ * to 1. To do that, run this command on device files created by this driver:
+ *
+ * v4l2-ctl --device /dev/videoX --set-ctrl=video_gop_size=1
+ *
+ * These issues are not decoding errors; all produced H.264 streams are decoded
+ * properly. Streams without P-frames don't have these artifacts so it's not
+ * analog-to-digital conversion issues nor internal memory errors; we conclude
+ * it's internal H.264 encoder issues.
+ * We cannot even check the original driver's behaviour because it has never
+ * worked properly at all in our development environment. So these issues may
+ * be actually related to firmware or hardware. However it may be that there's
+ * just some more register settings missing in the driver which would please
+ * the hardware.
+ * Manufacturer didn't help much on our inquiries, but feel free to disturb
+ * again the support of Intersil (owner of former Techwell).
+ */
+
+/* take first free /dev/videoX indexes by default */
+static unsigned int video_nr[] = {[0 ... (TW5864_INPUTS - 1)] = -1 };
+
+module_param_array(video_nr, int, NULL, 0444);
+MODULE_PARM_DESC(video_nr, "video devices numbers array");
+
+/*
+ * Please add any new PCI IDs to: http://pci-ids.ucw.cz.  This keeps
+ * the PCI ID database up to date.  Note that the entries must be
+ * added under vendor 0x1797 (Techwell Inc.) as subsystem IDs.
+ */
+static const struct pci_device_id tw5864_pci_tbl[] = {
+	{PCI_DEVICE(PCI_VENDOR_ID_TECHWELL, PCI_DEVICE_ID_TECHWELL_5864)},
+	{0,}
+};
+
+void tw5864_irqmask_apply(struct tw5864_dev *dev)
+{
+	tw_writel(TW5864_INTR_ENABLE_L, dev->irqmask & 0xffff);
+	tw_writel(TW5864_INTR_ENABLE_H, (dev->irqmask >> 16));
+}
+
+static void tw5864_interrupts_disable(struct tw5864_dev *dev)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->slock, flags);
+	dev->irqmask = 0;
+	tw5864_irqmask_apply(dev);
+	spin_unlock_irqrestore(&dev->slock, flags);
+}
+
+static void tw5864_timer_isr(struct tw5864_dev *dev);
+static void tw5864_h264_isr(struct tw5864_dev *dev);
+
+static irqreturn_t tw5864_isr(int irq, void *dev_id)
+{
+	struct tw5864_dev *dev = dev_id;
+	u32 status;
+
+	status = tw_readl(TW5864_INTR_STATUS_L) |
+		tw_readl(TW5864_INTR_STATUS_H) << 16;
+	if (!status)
+		return IRQ_NONE;
+
+	tw_writel(TW5864_INTR_CLR_L, 0xffff);
+	tw_writel(TW5864_INTR_CLR_H, 0xffff);
+
+	if (status & TW5864_INTR_VLC_DONE)
+		tw5864_h264_isr(dev);
+
+	if (status & TW5864_INTR_TIMER)
+		tw5864_timer_isr(dev);
+
+	if (!(status & (TW5864_INTR_TIMER | TW5864_INTR_VLC_DONE))) {
+		dev_dbg(&dev->pci->dev, "Unknown interrupt, status 0x%08X\n",
+			status);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void tw5864_h264_isr(struct tw5864_dev *dev)
+{
+	int channel = tw_readl(TW5864_DSP) & TW5864_DSP_ENC_CHN;
+	struct tw5864_input *input = &dev->inputs[channel];
+	int cur_frame_index, next_frame_index;
+	struct tw5864_h264_frame *cur_frame, *next_frame;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->slock, flags);
+
+	cur_frame_index = dev->h264_buf_w_index;
+	next_frame_index = (cur_frame_index + 1) % H264_BUF_CNT;
+	cur_frame = &dev->h264_buf[cur_frame_index];
+	next_frame = &dev->h264_buf[next_frame_index];
+
+	if (next_frame_index != dev->h264_buf_r_index) {
+		cur_frame->vlc_len = tw_readl(TW5864_VLC_LENGTH) << 2;
+		cur_frame->checksum = tw_readl(TW5864_VLC_CRC_REG);
+		cur_frame->input = input;
+		cur_frame->timestamp = ktime_get_ns();
+		cur_frame->seqno = input->frame_seqno;
+		cur_frame->gop_seqno = input->frame_gop_seqno;
+
+		dev->h264_buf_w_index = next_frame_index;
+		tasklet_schedule(&dev->tasklet);
+
+		cur_frame = next_frame;
+
+		spin_lock_irqsave(&input->slock, flags);
+		input->frame_seqno++;
+		input->frame_gop_seqno++;
+		if (input->frame_gop_seqno >= input->gop)
+			input->frame_gop_seqno = 0;
+		spin_unlock_irqrestore(&input->slock, flags);
+	} else {
+		dev_err(&dev->pci->dev,
+			"Skipped frame on input %d because all buffers busy\n",
+			channel);
+	}
+
+	dev->encoder_busy = 0;
+
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	tw_writel(TW5864_VLC_STREAM_BASE_ADDR, cur_frame->vlc.dma_addr);
+	tw_writel(TW5864_MV_STREAM_BASE_ADDR, cur_frame->mv.dma_addr);
+
+	/* Additional ack for this interrupt */
+	tw_writel(TW5864_VLC_DSP_INTR, 0x00000001);
+	tw_writel(TW5864_PCI_INTR_STATUS, TW5864_VLC_DONE_INTR);
+}
+
+static void tw5864_input_deadline_update(struct tw5864_input *input)
+{
+	input->new_frame_deadline = jiffies + msecs_to_jiffies(1000);
+}
+
+static void tw5864_timer_isr(struct tw5864_dev *dev)
+{
+	unsigned long flags;
+	int i;
+	int encoder_busy;
+
+	/* Additional ack for this interrupt */
+	tw_writel(TW5864_PCI_INTR_STATUS, TW5864_TIMER_INTR);
+
+	spin_lock_irqsave(&dev->slock, flags);
+	encoder_busy = dev->encoder_busy;
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	if (encoder_busy)
+		return;
+
+	/*
+	 * Traversing inputs in round-robin fashion, starting from next to the
+	 * last processed one
+	 */
+	for (i = 0; i < TW5864_INPUTS; i++) {
+		int next_input = (i + dev->next_input) % TW5864_INPUTS;
+		struct tw5864_input *input = &dev->inputs[next_input];
+		int raw_buf_id; /* id of internal buf with last raw frame */
+
+		spin_lock_irqsave(&input->slock, flags);
+		if (!input->enabled)
+			goto next;
+
+		/* Check if new raw frame is available */
+		raw_buf_id = tw_mask_shift_readl(TW5864_SENIF_ORG_FRM_PTR1, 0x3,
+						 2 * input->nr);
+
+		if (input->buf_id != raw_buf_id) {
+			input->buf_id = raw_buf_id;
+			tw5864_input_deadline_update(input);
+			spin_unlock_irqrestore(&input->slock, flags);
+
+			spin_lock_irqsave(&dev->slock, flags);
+			dev->encoder_busy = 1;
+			dev->next_input = (next_input + 1) % TW5864_INPUTS;
+			spin_unlock_irqrestore(&dev->slock, flags);
+
+			tw5864_request_encoded_frame(input);
+			break;
+		}
+
+		/* No new raw frame; check if channel is stuck */
+		if (time_is_after_jiffies(input->new_frame_deadline)) {
+			/* If stuck, request new raw frames again */
+			tw_mask_shift_writel(TW5864_ENC_BUF_PTR_REC1, 0x3,
+					     2 * input->nr, input->buf_id + 3);
+			tw5864_input_deadline_update(input);
+		}
+next:
+		spin_unlock_irqrestore(&input->slock, flags);
+	}
+}
+
+static int tw5864_initdev(struct pci_dev *pci_dev,
+			  const struct pci_device_id *pci_id)
+{
+	struct tw5864_dev *dev;
+	int err;
+
+	dev = devm_kzalloc(&pci_dev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	snprintf(dev->name, sizeof(dev->name), "tw5864:%s", pci_name(pci_dev));
+
+	err = v4l2_device_register(&pci_dev->dev, &dev->v4l2_dev);
+	if (err)
+		return err;
+
+	/* pci init */
+	dev->pci = pci_dev;
+	err = pci_enable_device(pci_dev);
+	if (err) {
+		dev_err(&dev->pci->dev, "pci_enable_device() failed\n");
+		goto unreg_v4l2;
+	}
+
+	pci_set_master(pci_dev);
+
+	err = pci_set_dma_mask(pci_dev, DMA_BIT_MASK(32));
+	if (err) {
+		dev_err(&dev->pci->dev, "32 bit PCI DMA is not supported\n");
+		goto disable_pci;
+	}
+
+	/* get mmio */
+	err = pci_request_regions(pci_dev, dev->name);
+	if (err) {
+		dev_err(&dev->pci->dev, "Cannot request regions for MMIO\n");
+		goto disable_pci;
+	}
+	dev->mmio = pci_ioremap_bar(pci_dev, 0);
+	if (!dev->mmio) {
+		err = -EIO;
+		dev_err(&dev->pci->dev, "can't ioremap() MMIO memory\n");
+		goto release_mmio;
+	}
+
+	spin_lock_init(&dev->slock);
+
+	dev_info(&pci_dev->dev, "TW5864 hardware version: %04x\n",
+		 tw_readl(TW5864_HW_VERSION));
+	dev_info(&pci_dev->dev, "TW5864 H.264 core version: %04x:%04x\n",
+		 tw_readl(TW5864_H264REV),
+		 tw_readl(TW5864_UNDECLARED_H264REV_PART2));
+
+	err = tw5864_video_init(dev, video_nr);
+	if (err)
+		goto unmap_mmio;
+
+	/* get irq */
+	err = devm_request_irq(&pci_dev->dev, pci_dev->irq, tw5864_isr,
+			       IRQF_SHARED, "tw5864", dev);
+	if (err < 0) {
+		dev_err(&dev->pci->dev, "can't get IRQ %d\n", pci_dev->irq);
+		goto fini_video;
+	}
+
+	dev_info(&pci_dev->dev, "Note: there are known video quality issues. For details\n");
+	dev_info(&pci_dev->dev, "see the comment in drivers/media/pci/tw5864/tw5864-core.c.\n");
+
+	return 0;
+
+fini_video:
+	tw5864_video_fini(dev);
+unmap_mmio:
+	iounmap(dev->mmio);
+release_mmio:
+	pci_release_regions(pci_dev);
+disable_pci:
+	pci_disable_device(pci_dev);
+unreg_v4l2:
+	v4l2_device_unregister(&dev->v4l2_dev);
+	return err;
+}
+
+static void tw5864_finidev(struct pci_dev *pci_dev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pci_dev);
+	struct tw5864_dev *dev =
+		container_of(v4l2_dev, struct tw5864_dev, v4l2_dev);
+
+	/* shutdown subsystems */
+	tw5864_interrupts_disable(dev);
+
+	/* unregister */
+	tw5864_video_fini(dev);
+
+	/* release resources */
+	iounmap(dev->mmio);
+	release_mem_region(pci_resource_start(pci_dev, 0),
+			   pci_resource_len(pci_dev, 0));
+
+	v4l2_device_unregister(&dev->v4l2_dev);
+	devm_kfree(&pci_dev->dev, dev);
+}
+
+static struct pci_driver tw5864_pci_driver = {
+	.name = "tw5864",
+	.id_table = tw5864_pci_tbl,
+	.probe = tw5864_initdev,
+	.remove = tw5864_finidev,
+};
+
+module_pci_driver(tw5864_pci_driver);

drivers/media/pci/tw5864/tw5864-h264.c

+/*
+ *  TW5864 driver - H.264 headers generation functions
+ *
+ *  Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.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.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#include <linux/log2.h>
+
+#include "tw5864.h"
+
+static u8 marker[] = { 0x00, 0x00, 0x00, 0x01 };
+
+/*
+ * Exponential-Golomb coding functions
+ *
+ * These functions are used for generation of H.264 bitstream headers.
+ *
+ * This code is derived from tw5864 reference driver by manufacturers, which
+ * itself apparently was derived from x264 project.
+ */
+
+/* Bitstream writing context */
+struct bs {
+	u8 *buf; /* pointer to buffer beginning */
+	u8 *buf_end; /* pointer to buffer end */
+	u8 *ptr; /* pointer to current byte in buffer */
+	unsigned int bits_left; /* number of available bits in current byte */
+};
+
+static void bs_init(struct bs *s, void *buf, int size)
+{
+	s->buf = buf;
+	s->ptr = buf;
+	s->buf_end = s->ptr + size;
+	s->bits_left = 8;
+}
+
+static int bs_len(struct bs *s)
+{
+	return s->ptr - s->buf;
+}
+
+static void bs_write(struct bs *s, int count, u32 bits)
+{
+	if (s->ptr >= s->buf_end - 4)
+		return;
+	while (count > 0) {
+		if (count < 32)
+			bits &= (1 << count) - 1;
+		if (count < s->bits_left) {
+			*s->ptr = (*s->ptr << count) | bits;
+			s->bits_left -= count;
+			break;
+		}
+		*s->ptr = (*s->ptr << s->bits_left) |
+			(bits >> (count - s->bits_left));
+		count -= s->bits_left;
+		s->ptr++;
+		s->bits_left = 8;
+	}
+}
+
+static void bs_write1(struct bs *s, u32 bit)
+{
+	if (s->ptr < s->buf_end) {
+		*s->ptr <<= 1;
+		*s->ptr |= bit;
+		s->bits_left--;
+		if (s->bits_left == 0) {
+			s->ptr++;
+			s->bits_left = 8;
+		}
+	}
+}
+
+static void bs_write_ue(struct bs *s, u32 val)
+{
+	if (val == 0) {
+		bs_write1(s, 1);
+	} else {
+		val++;
+		bs_write(s, 2 * fls(val) - 1, val);
+	}
+}
+
+static void bs_write_se(struct bs *s, int val)
+{
+	bs_write_ue(s, val <= 0 ? -val * 2 : val * 2 - 1);
+}
+
+static void bs_rbsp_trailing(struct bs *s)
+{
+	bs_write1(s, 1);
+	if (s->bits_left != 8)
+		bs_write(s, s->bits_left, 0x00);
+}
+
+/* H.264 headers generation functions */
+
+static int tw5864_h264_gen_sps_rbsp(u8 *buf, size_t size, int width, int height)
+{
+	struct bs bs, *s;
+
+	s = &bs;
+	bs_init(s, buf, size);
+	bs_write(s, 8, 0x42); /* profile_idc, baseline */
+	bs_write(s, 1, 1); /* constraint_set0_flag */
+	bs_write(s, 1, 1); /* constraint_set1_flag */
+	bs_write(s, 1, 0); /* constraint_set2_flag */
+	bs_write(s, 5, 0); /* reserved_zero_5bits */
+	bs_write(s, 8, 0x1e); /* level_idc */
+	bs_write_ue(s, 0); /* seq_parameter_set_id */
+	bs_write_ue(s, ilog2(MAX_GOP_SIZE) - 4); /* log2_max_frame_num_minus4 */
+	bs_write_ue(s, 0); /* pic_order_cnt_type */
+	/* log2_max_pic_order_cnt_lsb_minus4 */
+	bs_write_ue(s, ilog2(MAX_GOP_SIZE) - 4);
+	bs_write_ue(s, 1); /* num_ref_frames */
+	bs_write(s, 1, 0); /* gaps_in_frame_num_value_allowed_flag */
+	bs_write_ue(s, width / 16 - 1); /* pic_width_in_mbs_minus1 */
+	bs_write_ue(s, height / 16 - 1); /* pic_height_in_map_units_minus1 */
+	bs_write(s, 1, 1); /* frame_mbs_only_flag */
+	bs_write(s, 1, 0); /* direct_8x8_inference_flag */
+	bs_write(s, 1, 0); /* frame_cropping_flag */
+	bs_write(s, 1, 0); /* vui_parameters_present_flag */
+	bs_rbsp_trailing(s);
+	return bs_len(s);
+}
+
+static int tw5864_h264_gen_pps_rbsp(u8 *buf, size_t size, int qp)
+{
+	struct bs bs, *s;
+
+	s = &bs;
+	bs_init(s, buf, size);
+	bs_write_ue(s, 0); /* pic_parameter_set_id */
+	bs_write_ue(s, 0); /* seq_parameter_set_id */
+	bs_write(s, 1, 0); /* entropy_coding_mode_flag */
+	bs_write(s, 1, 0); /* pic_order_present_flag */
+	bs_write_ue(s, 0); /* num_slice_groups_minus1 */
+	bs_write_ue(s, 0); /* i_num_ref_idx_l0_active_minus1 */
+	bs_write_ue(s, 0); /* i_num_ref_idx_l1_active_minus1 */
+	bs_write(s, 1, 0); /* weighted_pred_flag */
+	bs_write(s, 2, 0); /* weighted_bipred_idc */
+	bs_write_se(s, qp - 26); /* pic_init_qp_minus26 */
+	bs_write_se(s, qp - 26); /* pic_init_qs_minus26 */
+	bs_write_se(s, 0); /* chroma_qp_index_offset */
+	bs_write(s, 1, 0); /* deblocking_filter_control_present_flag */
+	bs_write(s, 1, 0); /* constrained_intra_pred_flag */
+	bs_write(s, 1, 0); /* redundant_pic_cnt_present_flag */
+	bs_rbsp_trailing(s);
+	return bs_len(s);
+}
+
+static int tw5864_h264_gen_slice_head(u8 *buf, size_t size,
+				      unsigned int idr_pic_id,
+				      unsigned int frame_gop_seqno,
+				      int *tail_nb_bits, u8 *tail)
+{
+	struct bs bs, *s;
+	int is_i_frame = frame_gop_seqno == 0;
+
+	s = &bs;
+	bs_init(s, buf, size);
+	bs_write_ue(s, 0); /* first_mb_in_slice */
+	bs_write_ue(s, is_i_frame ? 2 : 5); /* slice_type - I or P */
+	bs_write_ue(s, 0); /* pic_parameter_set_id */
+	bs_write(s, ilog2(MAX_GOP_SIZE), frame_gop_seqno); /* frame_num */
+	if (is_i_frame)
+		bs_write_ue(s, idr_pic_id);
+
+	/* pic_order_cnt_lsb */
+	bs_write(s, ilog2(MAX_GOP_SIZE), frame_gop_seqno);
+
+	if (is_i_frame) {
+		bs_write1(s, 0); /* no_output_of_prior_pics_flag */
+		bs_write1(s, 0); /* long_term_reference_flag */
+	} else {
+		bs_write1(s, 0); /* num_ref_idx_active_override_flag */
+		bs_write1(s, 0); /* ref_pic_list_reordering_flag_l0 */
+		bs_write1(s, 0); /* adaptive_ref_pic_marking_mode_flag */
+	}
+
+	bs_write_se(s, 0); /* slice_qp_delta */
+
+	if (s->bits_left != 8) {
+		*tail = ((s->ptr[0]) << s->bits_left);
+		*tail_nb_bits = 8 - s->bits_left;
+	} else {
+		*tail = 0;
+		*tail_nb_bits = 0;
+	}
+
+	return bs_len(s);
+}
+
+void tw5864_h264_put_stream_header(u8 **buf, size_t *space_left, int qp,
+				   int width, int height)
+{
+	int nal_len;
+
+	/* SPS */
+	memcpy(*buf, marker, sizeof(marker));
+	*buf += 4;
+	*space_left -= 4;
+
+	**buf = 0x67; /* SPS NAL header */
+	*buf += 1;
+	*space_left -= 1;
+
+	nal_len = tw5864_h264_gen_sps_rbsp(*buf, *space_left, width, height);
+	*buf += nal_len;
+	*space_left -= nal_len;
+
+	/* PPS */
+	memcpy(*buf, marker, sizeof(marker));
+	*buf += 4;
+	*space_left -= 4;
+
+	**buf = 0x68; /* PPS NAL header */
+	*buf += 1;
+	*space_left -= 1;
+
+	nal_len = tw5864_h264_gen_pps_rbsp(*buf, *space_left, qp);
+	*buf += nal_len;
+	*space_left -= nal_len;
+}
+
+void tw5864_h264_put_slice_header(u8 **buf, size_t *space_left,
+				  unsigned int idr_pic_id,
+				  unsigned int frame_gop_seqno,
+				  int *tail_nb_bits, u8 *tail)
+{
+	int nal_len;
+
+	memcpy(*buf, marker, sizeof(marker));
+	*buf += 4;
+	*space_left -= 4;
+
+	/* Frame NAL header */
+	**buf = (frame_gop_seqno == 0) ? 0x25 : 0x21;
+	*buf += 1;
+	*space_left -= 1;
+
+	nal_len = tw5864_h264_gen_slice_head(*buf, *space_left, idr_pic_id,
+					     frame_gop_seqno, tail_nb_bits,
+					     tail);
+	*buf += nal_len;
+	*space_left -= nal_len;
+}

drivers/media/pci/tw5864/tw5864-util.c

+#include "tw5864.h"
+
+void tw5864_indir_writeb(struct tw5864_dev *dev, u16 addr, u8 data)
+{
+	int retries = 30000;
+
+	while (tw_readl(TW5864_IND_CTL) & BIT(31) && --retries)
+		;
+	if (!retries)
+		dev_err(&dev->pci->dev,
+			"tw_indir_writel() retries exhausted before writing\n");
+
+	tw_writel(TW5864_IND_DATA, data);
+	tw_writel(TW5864_IND_CTL, addr << 2 | TW5864_RW | TW5864_ENABLE);
+}
+
+u8 tw5864_indir_readb(struct tw5864_dev *dev, u16 addr)
+{
+	int retries = 30000;
+
+	while (tw_readl(TW5864_IND_CTL) & BIT(31) && --retries)
+		;
+	if (!retries)
+		dev_err(&dev->pci->dev,
+			"tw_indir_readl() retries exhausted before reading\n");
+
+	tw_writel(TW5864_IND_CTL, addr << 2 | TW5864_ENABLE);
+
+	retries = 30000;
+	while (tw_readl(TW5864_IND_CTL) & BIT(31) && --retries)
+		;
+	if (!retries)
+		dev_err(&dev->pci->dev,
+			"tw_indir_readl() retries exhausted at reading\n");
+
+	return tw_readl(TW5864_IND_DATA);
+}

drivers/media/pci/tw5864/tw5864.h

+/*
+ *  TW5864 driver  - common header file
+ *
+ *  Copyright (C) 2016 Bluecherry, LLC <maintainers@bluecherrydvr.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.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#include <linux/pci.h>
+#include <linux/videodev2.h>
+#include <linux/notifier.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "tw5864-reg.h"
+
+#define PCI_DEVICE_ID_TECHWELL_5864 0x5864
+
+#define TW5864_NORMS V4L2_STD_ALL
+
+/* ----------------------------------------------------------- */
+/* card configuration   */
+
+#define TW5864_INPUTS 4
+
+/* The TW5864 uses 192 (16x12) detection cells in full screen for motion
+ * detection. Each detection cell is composed of 44 pixels and 20 lines for
+ * NTSC and 24 lines for PAL.
+ */
+#define MD_CELLS_HOR 16
+#define MD_CELLS_VERT 12
+#define MD_CELLS (MD_CELLS_HOR * MD_CELLS_VERT)
+
+#define H264_VLC_BUF_SIZE 0x80000
+#define H264_MV_BUF_SIZE 0x2000 /* device writes 5396 bytes */
+#define QP_VALUE 28
+#define MAX_GOP_SIZE 255
+#define GOP_SIZE MAX_GOP_SIZE
+
+enum resolution {
+	D1 = 1,
+	HD1 = 2, /* half d1 - 360x(240|288) */
+	CIF = 3,
+	QCIF = 4,
+};
+
+/* ----------------------------------------------------------- */
+/* device / file handle status                                 */
+
+struct tw5864_dev; /* forward delclaration */
+
+/* buffer for one video/vbi/ts frame */
+struct tw5864_buf {
+	struct vb2_v4l2_buffer vb;
+	struct list_head list;
+
+	unsigned int size;
+};
+
+struct tw5864_dma_buf {
+	void *addr;
+	dma_addr_t dma_addr;
+};
+
+enum tw5864_vid_std {
+	STD_NTSC = 0, /* NTSC (M) */
+	STD_PAL = 1, /* PAL (B, D, G, H, I) */
+	STD_SECAM = 2, /* SECAM */
+	STD_NTSC443 = 3, /* NTSC4.43 */
+	STD_PAL_M = 4, /* PAL (M) */
+	STD_PAL_CN = 5, /* PAL (CN) */
+	STD_PAL_60 = 6, /* PAL 60 */
+	STD_INVALID = 7,
+	STD_AUTO = 7,
+};
+
+struct tw5864_input {
+	int nr; /* input number */
+	struct tw5864_dev *root;
+	struct mutex lock; /* used for vidq and vdev */
+	spinlock_t slock; /* used for sync between ISR, tasklet & V4L2 API */
+	struct video_device vdev;
+	struct v4l2_ctrl_handler hdl;
+	struct vb2_queue vidq;
+	struct list_head active;
+	enum resolution resolution;
+	unsigned int width, height;
+	unsigned int frame_seqno;
+	unsigned int frame_gop_seqno;
+	unsigned int h264_idr_pic_id;
+	int enabled;
+	enum tw5864_vid_std std;
+	v4l2_std_id v4l2_std;
+	int tail_nb_bits;
+	u8 tail;
+	u8 *buf_cur_ptr;
+	int buf_cur_space_left;
+
+	u32 reg_interlacing;
+	u32 reg_vlc;
+	u32 reg_dsp_codec;
+	u32 reg_dsp;
+	u32 reg_emu;
+	u32 reg_dsp_qp;
+	u32 reg_dsp_ref_mvp_lambda;
+	u32 reg_dsp_i4x4_weight;
+	u32 buf_id;
+
+	struct tw5864_buf *vb;
+
+	struct v4l2_ctrl *md_threshold_grid_ctrl;
+	u16 md_threshold_grid_values[12 * 16];
+	int qp;
+	int gop;
+
+	/*
+	 * In (1/MAX_FPS) units.
+	 * For max FPS (default), set to 1.
+	 * For 1 FPS, set to e.g. 32.
+	 */
+	int frame_interval;
+	unsigned long new_frame_deadline;
+};
+
+struct tw5864_h264_frame {
+	struct tw5864_dma_buf vlc;
+	struct tw5864_dma_buf mv;
+	int vlc_len;
+	u32 checksum;
+	struct tw5864_input *input;
+	u64 timestamp;
+	unsigned int seqno;
+	unsigned int gop_seqno;
+};
+
+/* global device status */
+struct tw5864_dev {
+	spinlock_t slock; /* used for sync between ISR, tasklet & V4L2 API */
+	struct v4l2_device v4l2_dev;
+	struct tw5864_input inputs[TW5864_INPUTS];
+#define H264_BUF_CNT 4
+	struct tw5864_h264_frame h264_buf[H264_BUF_CNT];
+	int h264_buf_r_index;
+	int h264_buf_w_index;
+
+	struct tasklet_struct tasklet;
+
+	int encoder_busy;
+	/* Input number to check next for ready raw picture (in RR fashion) */
+	int next_input;
+
+	/* pci i/o */
+	char name[64];
+	struct pci_dev *pci;
+	void __iomem *mmio;
+	u32 irqmask;
+};
+
+#define tw_readl(reg) readl(dev->mmio + reg)
+#define tw_mask_readl(reg, mask) \
+	(tw_readl(reg) & (mask))
+#define tw_mask_shift_readl(reg, mask, shift) \
+	(tw_mask_readl((reg), ((mask) << (shift))) >> (shift))
+
+#define tw_writel(reg, value) writel((value), dev->mmio + reg)
+#define tw_mask_writel(reg, mask, value) \
+	tw_writel(reg, (tw_readl(reg) & ~(mask)) | ((value) & (mask)))
+#define tw_mask_shift_writel(reg, mask, shift, value) \
+	tw_mask_writel((reg), ((mask) << (shift)), ((value) << (shift)))
+
+#define tw_setl(reg, bit) tw_writel((reg), tw_readl(reg) | (bit))
+#define tw_clearl(reg, bit) tw_writel((reg), tw_readl(reg) & ~(bit))
+
+u8 tw5864_indir_readb(struct tw5864_dev *dev, u16 addr);
+#define tw_indir_readb(addr) tw5864_indir_readb(dev, addr)
+void tw5864_indir_writeb(struct tw5864_dev *dev, u16 addr, u8 data);
+#define tw_indir_writeb(addr, data) tw5864_indir_writeb(dev, addr, data)
+
+void tw5864_irqmask_apply(struct tw5864_dev *dev);
+int tw5864_video_init(struct tw5864_dev *dev, int *video_nr);
+void tw5864_video_fini(struct tw5864_dev *dev);
+void tw5864_prepare_frame_headers(struct tw5864_input *input);
+void tw5864_h264_put_stream_header(u8 **buf, size_t *space_left, int qp,
+				   int width, int height);
+void tw5864_h264_put_slice_header(u8 **buf, size_t *space_left,
+				  unsigned int idr_pic_id,
+				  unsigned int frame_gop_seqno,
+				  int *tail_nb_bits, u8 *tail);
+void tw5864_request_encoded_frame(struct tw5864_input *input);